From e1289a171966abc82430b727c930f7575b19a0fd Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Mon, 31 Mar 2025 11:49:47 +0900 Subject: [PATCH 1/6] refactor using modern c++ Signed-off-by: Yutaka Kondo --- .../autoware/kalman_filter/kalman_filter.hpp | 13 +++++++++---- .../kalman_filter/time_delay_kalman_filter.hpp | 17 +++++++++++++---- .../src/kalman_filter.cpp | 2 +- .../src/time_delay_kalman_filter.cpp | 4 ++-- 4 files changed, 25 insertions(+), 11 deletions(-) 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..72f5295a43 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,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 @@ -125,7 +130,7 @@ class KalmanFilter * @param i index of kalman filter state * @return value of i's component of the kalman filter state x[i] */ - double getXelement(unsigned int i) const; + [[nodiscard]] double getXelement(unsigned int i) const; /** * @brief calculate kalman filter state and covariance by prediction model with A, B, Q matrix. @@ -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 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..a7066ba8ae 100644 --- a/common/autoware_kalman_filter/src/kalman_filter.cpp +++ b/common/autoware_kalman_filter/src/kalman_filter.cpp @@ -136,7 +136,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 (std::isnan(K.array()).any() || std::isinf(K.array()).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..1e49aa66bf 100644 --- a/common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp +++ b/common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp @@ -71,7 +71,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 +79,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; } From 0cb79508c42324ef7c7fab1cd34f883e95eaa5dd Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Mon, 31 Mar 2025 12:44:03 +0900 Subject: [PATCH 2/6] remove ctor/dtor Signed-off-by: Yutaka Kondo --- common/autoware_kalman_filter/src/kalman_filter.cpp | 8 +------- .../src/time_delay_kalman_filter.cpp | 4 ---- 2 files changed, 1 insertion(+), 11 deletions(-) diff --git a/common/autoware_kalman_filter/src/kalman_filter.cpp b/common/autoware_kalman_filter/src/kalman_filter.cpp index a7066ba8ae..220b42b025 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 (std::isnan(K.array()).any() || std::isinf(K.array()).any()) { + if (isnan(K.array()).any() || isinf(K.array()).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 1e49aa66bf..ad7cb253f8 100644 --- a/common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp +++ b/common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp @@ -18,10 +18,6 @@ namespace autoware::kalman_filter { -TimeDelayKalmanFilter::TimeDelayKalmanFilter() -{ -} - void TimeDelayKalmanFilter::init( const Eigen::MatrixXd & x, const Eigen::MatrixXd & P0, const int max_delay_step) { From 0249d15b209587756f86e751039f05d2d811c88e Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Mon, 31 Mar 2025 12:49:39 +0900 Subject: [PATCH 3/6] precommit Signed-off-by: Yutaka Kondo --- common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp | 1 + 1 file changed, 1 insertion(+) 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 ad7cb253f8..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,6 +15,7 @@ #include "autoware/kalman_filter/time_delay_kalman_filter.hpp" #include +#include namespace autoware::kalman_filter { From 6d3d046a141afd017eea3f6aa90b1972085977c1 Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Mon, 31 Mar 2025 15:53:44 +0900 Subject: [PATCH 4/6] use eigen methods Signed-off-by: Yutaka Kondo --- common/autoware_kalman_filter/src/kalman_filter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/autoware_kalman_filter/src/kalman_filter.cpp b/common/autoware_kalman_filter/src/kalman_filter.cpp index 220b42b025..c7d891daec 100644 --- a/common/autoware_kalman_filter/src/kalman_filter.cpp +++ b/common/autoware_kalman_filter/src/kalman_filter.cpp @@ -130,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; } From 5083bdfe42288914d836c1edc4b30000f4c96963 Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Tue, 1 Apr 2025 16:40:39 +0900 Subject: [PATCH 5/6] Update common/autoware_kalman_filter/include/autoware/kalman_filter/kalman_filter.hpp --- .../include/autoware/kalman_filter/kalman_filter.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 72f5295a43..67a5126d25 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 @@ -130,7 +130,7 @@ class KalmanFilter * @param i index of kalman filter state * @return value of i's component of the kalman filter state x[i] */ - [[nodiscard]] double getXelement(unsigned int i) const; + double getXelement(unsigned int i) const; /** * @brief calculate kalman filter state and covariance by prediction model with A, B, Q matrix. From c5d9479d65c132e7a703f2acc96f4ac90b27b98a Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Wed, 16 Apr 2025 12:04:34 +0900 Subject: [PATCH 6/6] not delete copy ctor Signed-off-by: Yutaka Kondo --- .../include/autoware/kalman_filter/kalman_filter.hpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) 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 67a5126d25..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 @@ -56,8 +56,9 @@ class KalmanFilter */ ~KalmanFilter() = default; - KalmanFilter(const KalmanFilter &) = delete; - KalmanFilter & operator=(const KalmanFilter &) = delete; + // 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;