refactor(autoware_kalman_filter): rewrite using modern C++ without API breakage

common/autoware_kalman_filter/include/autoware/kalman_filter/kalman_filter.hpp

@@ -34,7 +34,7 @@ class KalmanFilter
   /**
    * @brief No initialization constructor.
    */
-  KalmanFilter();
+  KalmanFilter() = default;
 
   /**
    * @brief constructor with initialization
@@ -54,7 +54,12 @@ class KalmanFilter
   /**
    * @brief destructor
    */
-  ~KalmanFilter();
+  ~KalmanFilter() = default;
+
+  KalmanFilter(const KalmanFilter &) = delete;
+  KalmanFilter & operator=(const KalmanFilter &) = delete;
+  KalmanFilter(KalmanFilter &&) noexcept = default;
+  KalmanFilter & operator=(KalmanFilter &&) noexcept = default;
 
   /**
    * @brief initialization of kalman filter
@@ -172,7 +177,7 @@ class KalmanFilter
    * @brief calculate kalman filter state by measurement model with y_pred, C and R matrix. This is
    * mainly for EKF with variable matrix.
    * @param y measured values
-   * @param y output values expected from measurement model
+   * @param y_pred output values expected from measurement model
    * @param C coefficient matrix of x for measurement model
    * @param R covariance matrix for measurement model
    * @return bool to check matrix operations are being performed properly

common/autoware_kalman_filter/include/autoware/kalman_filter/time_delay_kalman_filter.hpp

@@ -37,7 +37,14 @@ class TimeDelayKalmanFilter : public KalmanFilter
   /**
    * @brief No initialization constructor.
    */
-  TimeDelayKalmanFilter();
+  TimeDelayKalmanFilter() = default;
+
+  ~TimeDelayKalmanFilter() = default;
+
+  TimeDelayKalmanFilter(const TimeDelayKalmanFilter &) = delete;
+  TimeDelayKalmanFilter & operator=(const TimeDelayKalmanFilter &) = delete;
+  TimeDelayKalmanFilter(TimeDelayKalmanFilter &&) noexcept = default;
+  TimeDelayKalmanFilter & operator=(TimeDelayKalmanFilter &&) noexcept = default;
 
   /**
    * @brief initialization of kalman filter
@@ -64,6 +71,7 @@ class TimeDelayKalmanFilter : public KalmanFilter
    * @param x_next predicted state by prediction model
    * @param A coefficient matrix of x for process model
    * @param Q covariance matrix for process model
+   * @return bool to check matrix operations are being performed properly
    */
   bool predictWithDelay(
     const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A, const Eigen::MatrixXd & Q);
@@ -75,15 +83,16 @@ class TimeDelayKalmanFilter : public KalmanFilter
    * @param C coefficient matrix of x for measurement model
    * @param R covariance matrix for measurement model
    * @param delay_step measurement delay
+   * @return bool to check matrix operations are being performed properly
    */
   bool updateWithDelay(
     const Eigen::MatrixXd & y, const Eigen::MatrixXd & C, const Eigen::MatrixXd & R,
     const int delay_step);
 
 private:
-  int max_delay_step_;  //!< @brief maximum number of delay steps
-  int dim_x_;           //!< @brief dimension of latest state
-  int dim_x_ex_;        //!< @brief dimension of extended state with dime delay
+  int max_delay_step_{0};  //!< @brief maximum number of delay steps
+  int dim_x_{0};           //!< @brief dimension of latest state
+  int dim_x_ex_{0};        //!< @brief dimension of extended state with dime delay
 };
 }  // namespace autoware::kalman_filter
 #endif  // AUTOWARE__KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_

common/autoware_kalman_filter/src/kalman_filter.cpp

@@ -16,19 +16,13 @@
 
 namespace autoware::kalman_filter
 {
-KalmanFilter::KalmanFilter()
-{
-}
 KalmanFilter::KalmanFilter(
   const Eigen::MatrixXd & x, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B,
   const Eigen::MatrixXd & C, const Eigen::MatrixXd & Q, const Eigen::MatrixXd & R,
   const Eigen::MatrixXd & P)
 {
   init(x, A, B, C, Q, R, P);
 }
-KalmanFilter::~KalmanFilter()
-{
-}
 bool KalmanFilter::init(
   const Eigen::MatrixXd & x, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B,
   const Eigen::MatrixXd & C, const Eigen::MatrixXd & Q, const Eigen::MatrixXd & R,
@@ -136,7 +130,7 @@ bool KalmanFilter::update(
   const Eigen::MatrixXd PCT = P_ * C.transpose();
   const Eigen::MatrixXd K = PCT * ((R + C * PCT).inverse());
 
-  if (isnan(K.array()).any() || isinf(K.array()).any()) {
+  if (K.array().isNaN().any() || K.array().isInf().any()) {
     return false;
   }
 

common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp

@@ -15,13 +15,10 @@
 #include "autoware/kalman_filter/time_delay_kalman_filter.hpp"
 
 #include <iostream>
+#include <utility>
 
 namespace autoware::kalman_filter
 {
-TimeDelayKalmanFilter::TimeDelayKalmanFilter()
-{
-}
-
 void TimeDelayKalmanFilter::init(
   const Eigen::MatrixXd & x, const Eigen::MatrixXd & P0, const int max_delay_step)
 {
@@ -71,15 +68,15 @@ bool TimeDelayKalmanFilter::predictWithDelay(
   Eigen::MatrixXd x_tmp = Eigen::MatrixXd::Zero(dim_x_ex_, 1);
   x_tmp.block(0, 0, dim_x_, 1) = x_next;
   x_tmp.block(dim_x_, 0, d_dim_x, 1) = x_.block(0, 0, d_dim_x, 1);
-  x_ = x_tmp;
+  x_ = std::move(x_tmp);
 
   /* update P with delayed measurement A matrix structure */
   Eigen::MatrixXd P_tmp = Eigen::MatrixXd::Zero(dim_x_ex_, dim_x_ex_);
   P_tmp.block(0, 0, dim_x_, dim_x_) = A * P_.block(0, 0, dim_x_, dim_x_) * A.transpose() + Q;
   P_tmp.block(0, dim_x_, dim_x_, d_dim_x) = A * P_.block(0, 0, dim_x_, d_dim_x);
   P_tmp.block(dim_x_, 0, d_dim_x, dim_x_) = P_.block(0, 0, d_dim_x, dim_x_) * A.transpose();
   P_tmp.block(dim_x_, dim_x_, d_dim_x, d_dim_x) = P_.block(0, 0, d_dim_x, d_dim_x);
-  P_ = P_tmp;
+  P_ = std::move(P_tmp);
 
   return true;
 }