diff --git a/common/autoware_kalman_filter/include/autoware/kalman_filter/kalman_filter.hpp b/common/autoware_kalman_filter/include/autoware/kalman_filter/kalman_filter.hpp index 74db04f6e8..0045751ef2 100644 --- a/common/autoware_kalman_filter/include/autoware/kalman_filter/kalman_filter.hpp +++ b/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,13 @@ class KalmanFilter /** * @brief destructor */ - ~KalmanFilter(); + ~KalmanFilter() = default; + + // TODO(youtalk): Delete copy constructor and assignment operator + KalmanFilter(const KalmanFilter &) = default; + KalmanFilter & operator=(const KalmanFilter &) = default; + KalmanFilter(KalmanFilter &&) noexcept = default; + KalmanFilter & operator=(KalmanFilter &&) noexcept = default; /** * @brief initialization of kalman filter @@ -172,7 +178,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 diff --git a/common/autoware_kalman_filter/include/autoware/kalman_filter/time_delay_kalman_filter.hpp b/common/autoware_kalman_filter/include/autoware/kalman_filter/time_delay_kalman_filter.hpp index 80375b7579..ec08a51dd1 100644 --- a/common/autoware_kalman_filter/include/autoware/kalman_filter/time_delay_kalman_filter.hpp +++ b/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_ diff --git a/common/autoware_kalman_filter/src/kalman_filter.cpp b/common/autoware_kalman_filter/src/kalman_filter.cpp index bbd963675f..c7d891daec 100644 --- a/common/autoware_kalman_filter/src/kalman_filter.cpp +++ b/common/autoware_kalman_filter/src/kalman_filter.cpp @@ -16,9 +16,6 @@ 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, @@ -26,9 +23,6 @@ KalmanFilter::KalmanFilter( { 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; } diff --git a/common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp b/common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp index 4d1dd8f33b..c28d8ab9e1 100644 --- a/common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp +++ b/common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp @@ -15,13 +15,10 @@ #include "autoware/kalman_filter/time_delay_kalman_filter.hpp" #include +#include namespace autoware::kalman_filter { -TimeDelayKalmanFilter::TimeDelayKalmanFilter() -{ -} - void TimeDelayKalmanFilter::init( const Eigen::MatrixXd & x, const Eigen::MatrixXd & P0, const int max_delay_step) { @@ -71,7 +68,7 @@ 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_); @@ -79,7 +76,7 @@ bool TimeDelayKalmanFilter::predictWithDelay( 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; }