From 8a8fb276370e95bb9e31aec0a360d8064693d17b Mon Sep 17 00:00:00 2001 From: liu cui Date: Thu, 9 Jan 2025 09:47:46 +0800 Subject: [PATCH] feat(autoware_kalman_filter): move autoware_kalman_filter to autoware.core Signed-off-by: liu cui --- common/autoware_kalman_filter/CHANGELOG.rst | 55 ----- common/autoware_kalman_filter/CMakeLists.txt | 29 --- common/autoware_kalman_filter/README.md | 9 - .../autoware/kalman_filter/kalman_filter.hpp | 214 ------------------ .../time_delay_kalman_filter.hpp | 89 -------- common/autoware_kalman_filter/package.xml | 28 --- .../src/kalman_filter.cpp | 161 ------------- .../src/time_delay_kalman_filter.cpp | 109 --------- .../test/test_kalman_filter.cpp | 96 -------- .../test/test_time_delay_kalman_filter.cpp | 123 ---------- 10 files changed, 913 deletions(-) delete mode 100644 common/autoware_kalman_filter/CHANGELOG.rst delete mode 100644 common/autoware_kalman_filter/CMakeLists.txt delete mode 100644 common/autoware_kalman_filter/README.md delete mode 100644 common/autoware_kalman_filter/include/autoware/kalman_filter/kalman_filter.hpp delete mode 100644 common/autoware_kalman_filter/include/autoware/kalman_filter/time_delay_kalman_filter.hpp delete mode 100644 common/autoware_kalman_filter/package.xml delete mode 100644 common/autoware_kalman_filter/src/kalman_filter.cpp delete mode 100644 common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp delete mode 100644 common/autoware_kalman_filter/test/test_kalman_filter.cpp delete mode 100644 common/autoware_kalman_filter/test/test_time_delay_kalman_filter.cpp diff --git a/common/autoware_kalman_filter/CHANGELOG.rst b/common/autoware_kalman_filter/CHANGELOG.rst deleted file mode 100644 index a7e68f8843e74..0000000000000 --- a/common/autoware_kalman_filter/CHANGELOG.rst +++ /dev/null @@ -1,55 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_kalman_filter -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.40.0 (2024-12-12) -------------------- -* Merge branch 'main' into release-0.40.0 -* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" - This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* chore(package.xml): bump version to 0.39.0 (`#9587 `_) - * chore(package.xml): bump version to 0.39.0 - * fix: fix ticket links in CHANGELOG.rst - * fix: remove unnecessary diff - --------- - Co-authored-by: Yutaka Kondo -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* fix(cpplint): include what you use - common (`#9564 `_) -* 0.39.0 -* update changelog -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Fumiya Watanabe, M. Fatih Cırıt, Ryohsuke Mitsudome, Yutaka Kondo - -0.39.0 (2024-11-25) -------------------- -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* refactor(kalman_filter): prefix package and namespace with autoware (`#7787 `_) - * refactor(kalman_filter): prefix package and namespace with autoware - * move headers to include/autoware/ - * style(pre-commit): autofix - --------- - Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.26.0 (2024-04-03) -------------------- diff --git a/common/autoware_kalman_filter/CMakeLists.txt b/common/autoware_kalman_filter/CMakeLists.txt deleted file mode 100644 index 076d2d3cad4e8..0000000000000 --- a/common/autoware_kalman_filter/CMakeLists.txt +++ /dev/null @@ -1,29 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_kalman_filter) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(eigen3_cmake_module REQUIRED) -find_package(Eigen3 REQUIRED) - -include_directories( - SYSTEM - ${EIGEN3_INCLUDE_DIR} -) - -ament_auto_add_library(${PROJECT_NAME} SHARED - src/kalman_filter.cpp - src/time_delay_kalman_filter.cpp - include/autoware/kalman_filter/kalman_filter.hpp - include/autoware/kalman_filter/time_delay_kalman_filter.hpp -) - -if(BUILD_TESTING) - file(GLOB_RECURSE test_files test/*.cpp) - ament_add_ros_isolated_gtest(test_${PROJECT_NAME} ${test_files}) - - target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) -endif() - -ament_auto_package() diff --git a/common/autoware_kalman_filter/README.md b/common/autoware_kalman_filter/README.md deleted file mode 100644 index 7c0feb9c2a61a..0000000000000 --- a/common/autoware_kalman_filter/README.md +++ /dev/null @@ -1,9 +0,0 @@ -# kalman_filter - -## Purpose - -This common package contains the kalman filter with time delay and the calculation of the kalman filter. - -## Assumptions / Known limits - -TBD. 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 deleted file mode 100644 index 74db04f6e838b..0000000000000 --- a/common/autoware_kalman_filter/include/autoware/kalman_filter/kalman_filter.hpp +++ /dev/null @@ -1,214 +0,0 @@ -// Copyright 2018-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__KALMAN_FILTER__KALMAN_FILTER_HPP_ -#define AUTOWARE__KALMAN_FILTER__KALMAN_FILTER_HPP_ - -#include -#include - -namespace autoware::kalman_filter -{ - -/** - * @file kalman_filter.h - * @brief kalman filter class - * @author Takamasa Horibe - * @date 2019.05.01 - */ - -class KalmanFilter -{ -public: - /** - * @brief No initialization constructor. - */ - KalmanFilter(); - - /** - * @brief constructor with initialization - * @param x initial state - * @param A coefficient matrix of x for process model - * @param B coefficient matrix of u for process model - * @param C coefficient matrix of x for measurement model - * @param Q covariance matrix for process model - * @param R covariance matrix for measurement model - * @param P initial covariance of estimated state - */ - 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); - - /** - * @brief destructor - */ - ~KalmanFilter(); - - /** - * @brief initialization of kalman filter - * @param x initial state - * @param A coefficient matrix of x for process model - * @param B coefficient matrix of u for process model - * @param C coefficient matrix of x for measurement model - * @param Q covariance matrix for process model - * @param R covariance matrix for measurement model - * @param P initial covariance of estimated state - */ - bool 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, - const Eigen::MatrixXd & P); - - /** - * @brief initialization of kalman filter - * @param x initial state - * @param P initial covariance of estimated state - */ - bool init(const Eigen::MatrixXd & x, const Eigen::MatrixXd & P0); - - /** - * @brief set A of process model - * @param A coefficient matrix of x for process model - */ - void setA(const Eigen::MatrixXd & A); - - /** - * @brief set B of process model - * @param B coefficient matrix of u for process model - */ - void setB(const Eigen::MatrixXd & B); - - /** - * @brief set C of measurement model - * @param C coefficient matrix of x for measurement model - */ - void setC(const Eigen::MatrixXd & C); - - /** - * @brief set covariance matrix Q for process model - * @param Q covariance matrix for process model - */ - void setQ(const Eigen::MatrixXd & Q); - - /** - * @brief set covariance matrix R for measurement model - * @param R covariance matrix for measurement model - */ - void setR(const Eigen::MatrixXd & R); - - /** - * @brief get current kalman filter state - * @param x kalman filter state - */ - void getX(Eigen::MatrixXd & x) const; - - /** - * @brief get current kalman filter covariance - * @param P kalman filter covariance - */ - void getP(Eigen::MatrixXd & P) const; - - /** - * @brief get component of current kalman filter state - * @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; - - /** - * @brief calculate kalman filter state and covariance by prediction model with A, B, Q matrix. - * This is mainly for EKF with variable matrix. - * @param u input for model - * @param A coefficient matrix of x for process model - * @param B coefficient matrix of u for process model - * @param Q covariance matrix for process model - * @return bool to check matrix operations are being performed properly - */ - bool predict( - const Eigen::MatrixXd & u, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B, - const Eigen::MatrixXd & Q); - - /** - * @brief calculate kalman filter covariance with prediction model with x, A, Q matrix. This is - * mainly for EKF with variable matrix. - * @param x_next predicted state - * @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 predict( - const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A, const Eigen::MatrixXd & Q); - - /** - * @brief calculate kalman filter covariance with prediction model with x, A, Q matrix. This is - * mainly for EKF with variable matrix. - * @param x_next predicted state - * @param A coefficient matrix of x for process model - * @return bool to check matrix operations are being performed properly - */ - bool predict(const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A); - - /** - * @brief calculate kalman filter state by prediction model with A, B and Q being class member - * variables. - * @param u input for the model - * @return bool to check matrix operations are being performed properly - */ - bool predict(const Eigen::MatrixXd & u); - - /** - * @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 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 - */ - bool update( - const Eigen::MatrixXd & y, const Eigen::MatrixXd & y_pred, const Eigen::MatrixXd & C, - const Eigen::MatrixXd & R); - - /** - * @brief calculate kalman filter state by measurement model with C and R matrix. This is mainly - * for EKF with variable matrix. - * @param y measured values - * @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 - */ - bool update(const Eigen::MatrixXd & y, const Eigen::MatrixXd & C, const Eigen::MatrixXd & R); - - /** - * @brief calculate kalman filter state by measurement model with C and R being class member - * variables. - * @param y measured values - * @return bool to check matrix operations are being performed properly - */ - bool update(const Eigen::MatrixXd & y); - -protected: - Eigen::MatrixXd x_; //!< @brief current estimated state - Eigen::MatrixXd - A_; //!< @brief coefficient matrix of x for process model x[k+1] = A*x[k] + B*u[k] - Eigen::MatrixXd - B_; //!< @brief coefficient matrix of u for process model x[k+1] = A*x[k] + B*u[k] - Eigen::MatrixXd C_; //!< @brief coefficient matrix of x for measurement model y[k] = C * x[k] - Eigen::MatrixXd Q_; //!< @brief covariance matrix for process model x[k+1] = A*x[k] + B*u[k] - Eigen::MatrixXd R_; //!< @brief covariance matrix for measurement model y[k] = C * x[k] - Eigen::MatrixXd P_; //!< @brief covariance of estimated state -}; -} // namespace autoware::kalman_filter -#endif // AUTOWARE__KALMAN_FILTER__KALMAN_FILTER_HPP_ 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 deleted file mode 100644 index 80375b7579e62..0000000000000 --- a/common/autoware_kalman_filter/include/autoware/kalman_filter/time_delay_kalman_filter.hpp +++ /dev/null @@ -1,89 +0,0 @@ -// Copyright 2018-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_ -#define AUTOWARE__KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_ - -#include "autoware/kalman_filter/kalman_filter.hpp" - -#include -#include - -#include - -namespace autoware::kalman_filter -{ -/** - * @file time_delay_kalman_filter.h - * @brief kalman filter with delayed measurement class - * @author Takamasa Horibe - * @date 2019.05.01 - */ - -class TimeDelayKalmanFilter : public KalmanFilter -{ -public: - /** - * @brief No initialization constructor. - */ - TimeDelayKalmanFilter(); - - /** - * @brief initialization of kalman filter - * @param x initial state - * @param P0 initial covariance of estimated state - * @param max_delay_step Maximum number of delay steps, which determines the dimension of the - * extended kalman filter - */ - void init(const Eigen::MatrixXd & x, const Eigen::MatrixXd & P, const int max_delay_step); - - /** - * @brief get latest time estimated state - */ - Eigen::MatrixXd getLatestX() const; - - /** - * @brief get latest time estimation covariance - */ - Eigen::MatrixXd getLatestP() const; - - /** - * @brief calculate kalman filter covariance by precision model with time delay. This is mainly - * for EKF of nonlinear process model. - * @param x_next predicted state by prediction model - * @param A coefficient matrix of x for process model - * @param Q covariance matrix for process model - */ - bool predictWithDelay( - const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A, const Eigen::MatrixXd & Q); - - /** - * @brief calculate kalman filter covariance by measurement model with time delay. This is mainly - * for EKF of nonlinear process model. - * @param y measured values - * @param C coefficient matrix of x for measurement model - * @param R covariance matrix for measurement model - * @param delay_step measurement delay - */ - 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 -}; -} // namespace autoware::kalman_filter -#endif // AUTOWARE__KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_ diff --git a/common/autoware_kalman_filter/package.xml b/common/autoware_kalman_filter/package.xml deleted file mode 100644 index e51bb06e9de43..0000000000000 --- a/common/autoware_kalman_filter/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - - autoware_kalman_filter - 0.40.0 - The kalman filter package - Yukihiro Saito - Takeshi Ishita - Koji Minoda - Apache License 2.0 - - Takamasa Horibe - - ament_cmake_auto - autoware_cmake - - eigen - eigen3_cmake_module - - ament_cmake_cppcheck - ament_cmake_ros - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/common/autoware_kalman_filter/src/kalman_filter.cpp b/common/autoware_kalman_filter/src/kalman_filter.cpp deleted file mode 100644 index bbd963675f9e2..0000000000000 --- a/common/autoware_kalman_filter/src/kalman_filter.cpp +++ /dev/null @@ -1,161 +0,0 @@ -// Copyright 2018-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/kalman_filter/kalman_filter.hpp" - -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, - const Eigen::MatrixXd & P) -{ - if ( - x.cols() == 0 || x.rows() == 0 || A.cols() == 0 || A.rows() == 0 || B.cols() == 0 || - B.rows() == 0 || C.cols() == 0 || C.rows() == 0 || Q.cols() == 0 || Q.rows() == 0 || - R.cols() == 0 || R.rows() == 0 || P.cols() == 0 || P.rows() == 0) { - return false; - } - x_ = x; - A_ = A; - B_ = B; - C_ = C; - Q_ = Q; - R_ = R; - P_ = P; - return true; -} -bool KalmanFilter::init(const Eigen::MatrixXd & x, const Eigen::MatrixXd & P0) -{ - if (x.cols() == 0 || x.rows() == 0 || P0.cols() == 0 || P0.rows() == 0) { - return false; - } - x_ = x; - P_ = P0; - return true; -} - -void KalmanFilter::setA(const Eigen::MatrixXd & A) -{ - A_ = A; -} -void KalmanFilter::setB(const Eigen::MatrixXd & B) -{ - B_ = B; -} -void KalmanFilter::setC(const Eigen::MatrixXd & C) -{ - C_ = C; -} -void KalmanFilter::setQ(const Eigen::MatrixXd & Q) -{ - Q_ = Q; -} -void KalmanFilter::setR(const Eigen::MatrixXd & R) -{ - R_ = R; -} -void KalmanFilter::getX(Eigen::MatrixXd & x) const -{ - x = x_; -} -void KalmanFilter::getP(Eigen::MatrixXd & P) const -{ - P = P_; -} -double KalmanFilter::getXelement(unsigned int i) const -{ - return x_(i); -} - -bool KalmanFilter::predict( - const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A, const Eigen::MatrixXd & Q) -{ - if ( - x_.rows() != x_next.rows() || A.cols() != P_.rows() || Q.cols() != Q.rows() || - A.rows() != Q.cols()) { - return false; - } - x_ = x_next; - P_ = A * P_ * A.transpose() + Q; - return true; -} -bool KalmanFilter::predict(const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A) -{ - return predict(x_next, A, Q_); -} - -bool KalmanFilter::predict( - const Eigen::MatrixXd & u, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B, - const Eigen::MatrixXd & Q) -{ - if (A.cols() != x_.rows() || B.cols() != u.rows()) { - return false; - } - const Eigen::MatrixXd x_next = A * x_ + B * u; - return predict(x_next, A, Q); -} -bool KalmanFilter::predict(const Eigen::MatrixXd & u) -{ - return predict(u, A_, B_, Q_); -} - -bool KalmanFilter::update( - const Eigen::MatrixXd & y, const Eigen::MatrixXd & y_pred, const Eigen::MatrixXd & C, - const Eigen::MatrixXd & R) -{ - if ( - P_.cols() != C.cols() || R.rows() != R.cols() || R.rows() != C.rows() || - y.rows() != y_pred.rows() || y.rows() != C.rows()) { - return false; - } - 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()) { - return false; - } - - x_ = x_ + K * (y - y_pred); - P_ = P_ - K * (C * P_); - return true; -} - -bool KalmanFilter::update( - const Eigen::MatrixXd & y, const Eigen::MatrixXd & C, const Eigen::MatrixXd & R) -{ - if (C.cols() != x_.rows()) { - return false; - } - const Eigen::MatrixXd y_pred = C * x_; - return update(y, y_pred, C, R); -} -bool KalmanFilter::update(const Eigen::MatrixXd & y) -{ - return update(y, C_, R_); -} -} // namespace autoware::kalman_filter diff --git a/common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp b/common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp deleted file mode 100644 index 4d1dd8f33b7a6..0000000000000 --- a/common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp +++ /dev/null @@ -1,109 +0,0 @@ -// Copyright 2018-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/kalman_filter/time_delay_kalman_filter.hpp" - -#include - -namespace autoware::kalman_filter -{ -TimeDelayKalmanFilter::TimeDelayKalmanFilter() -{ -} - -void TimeDelayKalmanFilter::init( - const Eigen::MatrixXd & x, const Eigen::MatrixXd & P0, const int max_delay_step) -{ - max_delay_step_ = max_delay_step; - dim_x_ = x.rows(); - dim_x_ex_ = dim_x_ * max_delay_step; - - x_ = Eigen::MatrixXd::Zero(dim_x_ex_, 1); - P_ = Eigen::MatrixXd::Zero(dim_x_ex_, dim_x_ex_); - - for (int i = 0; i < max_delay_step_; ++i) { - x_.block(i * dim_x_, 0, dim_x_, 1) = x; - P_.block(i * dim_x_, i * dim_x_, dim_x_, dim_x_) = P0; - } -} - -Eigen::MatrixXd TimeDelayKalmanFilter::getLatestX() const -{ - return x_.block(0, 0, dim_x_, 1); -} - -Eigen::MatrixXd TimeDelayKalmanFilter::getLatestP() const -{ - return P_.block(0, 0, dim_x_, dim_x_); -} - -bool TimeDelayKalmanFilter::predictWithDelay( - const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A, const Eigen::MatrixXd & Q) -{ - /* - * time delay model: - * - * [A 0 0] [P11 P12 P13] [Q 0 0] - * A = [I 0 0], P = [P21 P22 P23], Q = [0 0 0] - * [0 I 0] [P31 P32 P33] [0 0 0] - * - * covariance calculation in prediction : P = A * P * A' + Q - * - * [A*P11*A'*+Q A*P11 A*P12] - * P = [ P11*A' P11 P12] - * [ P21*A' P21 P22] - */ - - const int d_dim_x = dim_x_ex_ - dim_x_; - - /* slide states in the time direction */ - 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; - - /* 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; - - return true; -} - -bool TimeDelayKalmanFilter::updateWithDelay( - const Eigen::MatrixXd & y, const Eigen::MatrixXd & C, const Eigen::MatrixXd & R, - const int delay_step) -{ - if (delay_step >= max_delay_step_) { - std::cerr << "delay step is larger than max_delay_step. ignore update." << std::endl; - return false; - } - - const int dim_y = y.rows(); - - /* set measurement matrix */ - Eigen::MatrixXd C_ex = Eigen::MatrixXd::Zero(dim_y, dim_x_ex_); - C_ex.block(0, dim_x_ * delay_step, dim_y, dim_x_) = C; - - /* update */ - if (!update(y, C_ex, R)) { - return false; - } - - return true; -} -} // namespace autoware::kalman_filter diff --git a/common/autoware_kalman_filter/test/test_kalman_filter.cpp b/common/autoware_kalman_filter/test/test_kalman_filter.cpp deleted file mode 100644 index 34e23ef9d06e2..0000000000000 --- a/common/autoware_kalman_filter/test/test_kalman_filter.cpp +++ /dev/null @@ -1,96 +0,0 @@ -// Copyright 2023 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/kalman_filter/kalman_filter.hpp" - -#include - -using autoware::kalman_filter::KalmanFilter; - -TEST(kalman_filter, kf) -{ - KalmanFilter kf_; - - Eigen::MatrixXd x_t(2, 1); - x_t << 1, 2; - - Eigen::MatrixXd P_t(2, 2); - P_t << 1, 0, 0, 1; - - Eigen::MatrixXd Q_t(2, 2); - Q_t << 0.01, 0, 0, 0.01; - - Eigen::MatrixXd R_t(2, 2); - R_t << 0.09, 0, 0, 0.09; - - Eigen::MatrixXd C_t(2, 2); - C_t << 1, 0, 0, 1; - - Eigen::MatrixXd A_t(2, 2); - A_t << 1, 0, 0, 1; - - Eigen::MatrixXd B_t(2, 2); - B_t << 1, 0, 0, 1; - - // Initialize the filter and check if initialization was successful - EXPECT_TRUE(kf_.init(x_t, A_t, B_t, C_t, Q_t, R_t, P_t)); - - // Perform prediction - Eigen::MatrixXd u_t(2, 1); - u_t << 0.1, 0.1; - EXPECT_TRUE(kf_.predict(u_t)); - - // Check the updated state and covariance matrix - Eigen::MatrixXd x_predict_expected = A_t * x_t + B_t * u_t; - Eigen::MatrixXd P_predict_expected = A_t * P_t * A_t.transpose() + Q_t; - - Eigen::MatrixXd x_predict; - kf_.getX(x_predict); - Eigen::MatrixXd P_predict; - kf_.getP(P_predict); - - EXPECT_NEAR(x_predict(0, 0), x_predict_expected(0, 0), 1e-5); - EXPECT_NEAR(x_predict(1, 0), x_predict_expected(1, 0), 1e-5); - EXPECT_NEAR(P_predict(0, 0), P_predict_expected(0, 0), 1e-5); - EXPECT_NEAR(P_predict(1, 1), P_predict_expected(1, 1), 1e-5); - - // Perform update - Eigen::MatrixXd y_t(2, 1); - y_t << 1.05, 2.05; - EXPECT_TRUE(kf_.update(y_t)); - - // Check the updated state and covariance matrix - const Eigen::MatrixXd PCT_t = P_predict_expected * C_t.transpose(); - const Eigen::MatrixXd K_t = PCT_t * ((R_t + C_t * PCT_t).inverse()); - const Eigen::MatrixXd y_pred = C_t * x_predict_expected; - Eigen::MatrixXd x_update_expected = x_predict_expected + K_t * (y_t - y_pred); - Eigen::MatrixXd P_update_expected = P_predict_expected - K_t * (C_t * P_predict_expected); - - Eigen::MatrixXd x_update; - kf_.getX(x_update); - Eigen::MatrixXd P_update; - kf_.getP(P_update); - - EXPECT_NEAR(x_update(0, 0), x_update_expected(0, 0), 1e-5); - EXPECT_NEAR(x_update(1, 0), x_update_expected(1, 0), 1e-5); - EXPECT_NEAR(P_update(0, 0), P_update_expected(0, 0), 1e-5); - EXPECT_NEAR(P_update(1, 1), P_update_expected(1, 1), 1e-5); -} - -int main(int argc, char * argv[]) -{ - testing::InitGoogleTest(&argc, argv); - bool result = RUN_ALL_TESTS(); - return result; -} diff --git a/common/autoware_kalman_filter/test/test_time_delay_kalman_filter.cpp b/common/autoware_kalman_filter/test/test_time_delay_kalman_filter.cpp deleted file mode 100644 index 50c22fae123bc..0000000000000 --- a/common/autoware_kalman_filter/test/test_time_delay_kalman_filter.cpp +++ /dev/null @@ -1,123 +0,0 @@ -// Copyright 2023 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/kalman_filter/time_delay_kalman_filter.hpp" - -#include - -using autoware::kalman_filter::TimeDelayKalmanFilter; - -TEST(time_delay_kalman_filter, td_kf) -{ - TimeDelayKalmanFilter td_kf_; - - Eigen::MatrixXd x_t(3, 1); - x_t << 1.0, 2.0, 3.0; - Eigen::MatrixXd P_t(3, 3); - P_t << 0.1, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.3; - const int max_delay_step = 5; - const int dim_x = x_t.rows(); - const int dim_x_ex = dim_x * max_delay_step; - // Initialize the filter - td_kf_.init(x_t, P_t, max_delay_step); - - // Check if initialization was successful - Eigen::MatrixXd x_init = td_kf_.getLatestX(); - Eigen::MatrixXd P_init = td_kf_.getLatestP(); - Eigen::MatrixXd x_ex_t = Eigen::MatrixXd::Zero(dim_x_ex, 1); - Eigen::MatrixXd P_ex_t = Eigen::MatrixXd::Zero(dim_x_ex, dim_x_ex); - for (int i = 0; i < max_delay_step; ++i) { - x_ex_t.block(i * dim_x, 0, dim_x, 1) = x_t; - P_ex_t.block(i * dim_x, i * dim_x, dim_x, dim_x) = P_t; - } - - EXPECT_EQ(x_init.rows(), 3); - EXPECT_EQ(x_init.cols(), 1); - EXPECT_EQ(P_init.rows(), 3); - EXPECT_EQ(P_init.cols(), 3); - EXPECT_NEAR(x_init(0, 0), x_t(0, 0), 1e-5); - EXPECT_NEAR(x_init(1, 0), x_t(1, 0), 1e-5); - EXPECT_NEAR(x_init(2, 0), x_t(2, 0), 1e-5); - EXPECT_NEAR(P_init(0, 0), P_t(0, 0), 1e-5); - EXPECT_NEAR(P_init(1, 1), P_t(1, 1), 1e-5); - EXPECT_NEAR(P_init(2, 2), P_t(2, 2), 1e-5); - - // Define prediction parameters - Eigen::MatrixXd A_t(3, 3); - A_t << 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 2.0; - Eigen::MatrixXd Q_t(3, 3); - Q_t << 0.01, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, 0.03; - Eigen::MatrixXd x_next(3, 1); - x_next << 2.0, 4.0, 6.0; - - // Perform prediction - EXPECT_TRUE(td_kf_.predictWithDelay(x_next, A_t, Q_t)); - - // Check the prediction state and covariance matrix - Eigen::MatrixXd x_predict = td_kf_.getLatestX(); - Eigen::MatrixXd P_predict = td_kf_.getLatestP(); - Eigen::MatrixXd x_tmp = Eigen::MatrixXd::Zero(dim_x_ex, 1); - x_tmp.block(0, 0, dim_x, 1) = A_t * x_t; - x_tmp.block(dim_x, 0, dim_x_ex - dim_x, 1) = x_ex_t.block(0, 0, dim_x_ex - dim_x, 1); - x_ex_t = x_tmp; - Eigen::MatrixXd x_predict_expected = x_ex_t.block(0, 0, dim_x, 1); - Eigen::MatrixXd P_tmp = Eigen::MatrixXd::Zero(dim_x_ex, dim_x_ex); - P_tmp.block(0, 0, dim_x, dim_x) = A_t * P_ex_t.block(0, 0, dim_x, dim_x) * A_t.transpose() + Q_t; - P_tmp.block(0, dim_x, dim_x, dim_x_ex - dim_x) = - A_t * P_ex_t.block(0, 0, dim_x, dim_x_ex - dim_x); - P_tmp.block(dim_x, 0, dim_x_ex - dim_x, dim_x) = - P_ex_t.block(0, 0, dim_x_ex - dim_x, dim_x) * A_t.transpose(); - P_tmp.block(dim_x, dim_x, dim_x_ex - dim_x, dim_x_ex - dim_x) = - P_ex_t.block(0, 0, dim_x_ex - dim_x, dim_x_ex - dim_x); - P_ex_t = P_tmp; - Eigen::MatrixXd P_predict_expected = P_ex_t.block(0, 0, dim_x, dim_x); - EXPECT_NEAR(x_predict(0, 0), x_predict_expected(0, 0), 1e-5); - EXPECT_NEAR(x_predict(1, 0), x_predict_expected(1, 0), 1e-5); - EXPECT_NEAR(x_predict(2, 0), x_predict_expected(2, 0), 1e-5); - EXPECT_NEAR(P_predict(0, 0), P_predict_expected(0, 0), 1e-5); - EXPECT_NEAR(P_predict(1, 1), P_predict_expected(1, 1), 1e-5); - EXPECT_NEAR(P_predict(2, 2), P_predict_expected(2, 2), 1e-5); - - // Define update parameters - Eigen::MatrixXd C_t(3, 3); - C_t << 0.5, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.5; - Eigen::MatrixXd R_t(3, 3); - R_t << 0.001, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.003; - Eigen::MatrixXd y_t(3, 1); - y_t << 1.05, 2.05, 3.05; - const int delay_step = 2; // Choose an appropriate delay step - const int dim_y = y_t.rows(); - - // Perform update - EXPECT_TRUE(td_kf_.updateWithDelay(y_t, C_t, R_t, delay_step)); - - // Check the updated state and covariance matrix - Eigen::MatrixXd x_update = td_kf_.getLatestX(); - Eigen::MatrixXd P_update = td_kf_.getLatestP(); - - Eigen::MatrixXd C_ex_t = Eigen::MatrixXd::Zero(dim_y, dim_x_ex); - const Eigen::MatrixXd PCT_t = P_ex_t * C_ex_t.transpose(); - const Eigen::MatrixXd K_t = PCT_t * ((R_t + C_ex_t * PCT_t).inverse()); - const Eigen::MatrixXd y_pred = C_ex_t * x_ex_t; - x_ex_t = x_ex_t + K_t * (y_t - y_pred); - P_ex_t = P_ex_t - K_t * (C_ex_t * P_ex_t); - Eigen::MatrixXd x_update_expected = x_ex_t.block(0, 0, dim_x, 1); - Eigen::MatrixXd P_update_expected = P_ex_t.block(0, 0, dim_x, dim_x); - EXPECT_NEAR(x_update(0, 0), x_update_expected(0, 0), 1e-5); - EXPECT_NEAR(x_update(1, 0), x_update_expected(1, 0), 1e-5); - EXPECT_NEAR(x_update(2, 0), x_update_expected(2, 0), 1e-5); - EXPECT_NEAR(P_update(0, 0), P_update_expected(0, 0), 1e-5); - EXPECT_NEAR(P_update(1, 1), P_update_expected(1, 1), 1e-5); - EXPECT_NEAR(P_update(2, 2), P_update_expected(2, 2), 1e-5); -}