diff --git a/common/autoware_kalman_filter/CMakeLists.txt b/common/autoware_kalman_filter/CMakeLists.txt new file mode 100644 index 00000000..076d2d3c --- /dev/null +++ b/common/autoware_kalman_filter/CMakeLists.txt @@ -0,0 +1,29 @@ +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 new file mode 100644 index 00000000..daa0066f --- /dev/null +++ b/common/autoware_kalman_filter/README.md @@ -0,0 +1,288 @@ +# kalman_filter + +## Overview + +This package contains the kalman filter with time delay and the calculation of the kalman filter. + +## Design + +The Kalman filter is a recursive algorithm used to estimate the state of a dynamic system. The Time Delay Kalman filter is based on the standard Kalman filter and takes into account possible time delays in the measured values. + +### Standard Kalman Filter + +#### System Model + +Assume that the system can be represented by the following linear discrete model: + +$$ +x_{k} = A x_{k-1} + B u_{k} \\ +y_{k} = C x_{k-1} +$$ + +where, + +- $x_k$ is the state vector at time $k$. +- $u_k$ is the control input vector at time $k$. +- $y_k$ is the measurement vector at time $k$. +- $A$ is the state transition matrix. +- $B$ is the control input matrix. +- $C$ is the measurement matrix. + +#### Prediction Step + +The prediction step consists of updating the state and covariance matrices: + +$$ +x_{k|k-1} = A x_{k-1|k-1} + B u_{k} \\ +P_{k|k-1} = A P_{k-1|k-1} A^{T} + Q +$$ + +where, + +- $x_{k|k-1}$ is the priori state estimate. +- $P_{k|k-1}$ is the priori covariance matrix. + +#### Update Step + +When the measurement value \( y_k \) is received, the update steps are as follows: + +$$ +K_k = P_{k|k-1} C^{T} (C P_{k|k-1} C^{T} + R)^{-1} \\ +x_{k|k} = x_{k|k-1} + K_k (y_{k} - C x_{k|k-1}) \\ +P_{k|k} = (I - K_k C) P_{k|k-1} +$$ + +where, + +- $K_k$ is the Kalman gain. +- $x_{k|k}$ is the posterior state estimate. +- $P_{k|k}$ is the posterior covariance matrix. + +### Extension to Time Delay Kalman Filter + +For the Time Delay Kalman filter, it is assumed that there may be a maximum delay of step ($d$) in the measured value. To handle this delay, we extend the state vector to: + +$$ +(x_{k})_e = \begin{bmatrix} +x_k \\ +x_{k-1} \\ +\vdots \\ +x_{k-d+1} +\end{bmatrix} +$$ + +The corresponding state transition matrix ($A_e$) and process noise covariance matrix ($Q_e$) are also expanded: + +$$ +A_e = \begin{bmatrix} +A & 0 & 0 & \cdots & 0 \\ +I & 0 & 0 & \cdots & 0 \\ +0 & I & 0 & \cdots & 0 \\ +\vdots & \vdots & \vdots & \ddots & \vdots \\ +0 & 0 & 0 & \cdots & 0 +\end{bmatrix}, \quad +Q_e = \begin{bmatrix} +Q & 0 & 0 & \cdots & 0 \\ +0 & 0 & 0 & \cdots & 0 \\ +0 & 0 & 0 & \cdots & 0 \\ +\vdots & \vdots & \vdots & \ddots & \vdots \\ +0 & 0 & 0 & \cdots & 0 +\end{bmatrix} +$$ + +#### Prediction Step + +The prediction step consists of updating the extended state and covariance matrices. + +Update extension status: + +$$ +(x_{k|k-1})_e = \begin{bmatrix} +A & 0 & 0 & \cdots & 0 \\ +I & 0 & 0 & \cdots & 0 \\ +0 & I & 0 & \cdots & 0 \\ +\vdots & \vdots & \vdots & \ddots & \vdots \\ +0 & 0 & 0 & \cdots & 0 +\end{bmatrix} +\begin{bmatrix} +x_{k-1|k-1} \\ +x_{k-2|k-1} \\ +\vdots \\ +x_{k-d|k-1} +\end{bmatrix} +$$ + +Update extended covariance matrix: + +$$ +(P_{k|k-1})_e = \begin{bmatrix} +A & 0 & 0 & \cdots & 0 \\ +I & 0 & 0 & \cdots & 0 \\ +0 & I & 0 & \cdots & 0 \\ +\vdots & \vdots & \vdots & \ddots & \vdots \\ +0 & 0 & 0 & \cdots & 0 +\end{bmatrix} +\begin{bmatrix} +P_{k-1|k-1}^{(1)} & P_{k-1|k-1}^{(1,2)} & \cdots & P_{k-1|k-1}^{(1,d)} \\ +P_{k-1|k-1}^{(2,1)} & P_{k-1|k-1}^{(2)} & \cdots & P_{k-1|k-1}^{(2,d)} \\ +\vdots & \vdots & \ddots & \vdots \\ +P_{k-1|k-1}^{(d,1)} & P_{k-1|k-1}^{(d,2)} & \cdots & P_{k-1|k-1}^{(d)} +\end{bmatrix} +\begin{bmatrix} + A^T & I & 0 & \cdots & 0 \\ + 0 & 0 & I & \cdots & 0 \\ + 0 & 0 & 0 & \cdots & 0 \\ + \vdots & \vdots & \vdots & \ddots & \vdots \\ + 0 & 0 & 0 & \cdots & 0 + \end{bmatrix} + + \begin{bmatrix} + Q & 0 & 0 & \cdots & 0 \\ + 0 & 0 & 0 & \cdots & 0 \\ + 0 & 0 & 0 & \cdots & 0 \\ + \vdots & \vdots & \vdots & \ddots & \vdots \\ + 0 & 0 & 0 & \cdots & 0 + \end{bmatrix} +$$ + +$\Longrightarrow$ + +$$ +(P_{k|k-1})_e = \begin{bmatrix} A P_{k-1|k-1}^{(1)} A^T + Q & A P_{k-1|k-1}^{(1,2)} & \cdots & A P_{k-1|k-1}^{(1,d)} \\ P_{k-1|k-1}^{(2,1)} A^T & P_{k-1|k-1}^{(2)} & \cdots & P_{k-1|k-1}^{(2,d)} \\ \vdots & \vdots & \ddots & \vdots \\ P_{k-1|k-1}^{(d,1)} A^T & P_{k-1|k-1}^{(d,2)} & \cdots & P_{k-1|k-1}^{(d)} \end{bmatrix} +$$ + +where, + +- $(x_{k|k-1})_e$ is the priori extended state estimate. +- $(P_{k|k-1})_e$ is the priori extended covariance matrix. + +#### Update Step + +When receiving the measurement value ( $y_{k}$ ) with a delay of ( $ds$ ), the update steps are as follows: + +Update kalman gain: + +$$ +K_k = \begin{bmatrix} +P_{k|k-1}^{(1)} C^T \\ +P_{k|k-1}^{(2)} C^T \\ +\vdots \\ +P_{k|k-1}^{(ds)} C^T \\ +\vdots \\ +P_{k|k-1}^{(d)} C^T +\end{bmatrix} +(C P_{k|k-1}^{(ds)} C^T + R)^{-1} +$$ + +Update extension status: + +$$ +(x_{k|k})_e = \begin{bmatrix} +x_{k|k-1} \\ +x_{k-1|k-1} \\ +\vdots \\ +x_{k-d+1|k-1} +\end{bmatrix} + +\begin{bmatrix} +K_k^{(1)} \\ +K_k^{(2)} \\ +\vdots \\ +K_k^{(ds)} \\ +\vdots \\ +K_k^{(d)} +\end{bmatrix} (y_k - C x_{k-ds|k-1}) +$$ + +Update extended covariance matrix: + +$$ + (P_{k|k})_e = \left(I - + \begin{bmatrix} + K_k^{(1)} C \\ + K_k^{(2)} C \\ + \vdots \\ + K_k^{(ds)} C \\ + \vdots \\ + K_k^{(d)} C + \end{bmatrix}\right) + \begin{bmatrix} + P_{k|k-1}^{(1)} & P_{k|k-1}^{(1,2)} & \cdots & P_{k|k-1}^{(1,d)} \\ + P_{k|k-1}^{(2,1)} & P_{k|k-1}^{(2)} & \cdots & P_{k|k-1}^{(2,d)} \\ + \vdots & \vdots & \ddots & \vdots \\ + P_{k|k-1}^{(d,1)} & P_{k|k-1}^{(d,2)} & \cdots & P_{k|k-1}^{(d)} + \end{bmatrix} +$$ + +where, + +- $K_k$ is the Kalman gain. +- $(x_{k|k})_e$ is the posterior extended state estimate. +- $(P_{k|k})_e$ is the posterior extended covariance matrix. +- $C$ is the measurement matrix, which only applies to the delayed state part. + +## Example Usage + +This section describes Example Usage of KalmanFilter. + +- Initialization + +```cpp +#include "autoware/kalman_filter/kalman_filter.hpp" + +// Define system parameters +int dim_x = 2; // state vector dimensions +int dim_y = 1; // measure vector dimensions + +// Initial state +Eigen::MatrixXd x0 = Eigen::MatrixXd::Zero(dim_x, 1); +x0 << 0.0, 0.0; + +// Initial covariance matrix +Eigen::MatrixXd P0 = Eigen::MatrixXd::Identity(dim_x, dim_x); +P0 *= 100.0; + +// Define state transition matrix +Eigen::MatrixXd A = Eigen::MatrixXd::Identity(dim_x, dim_x); +A(0, 1) = 1.0; + +// Define measurement matrix +Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, dim_x); +C(0, 0) = 1.0; + +// Define process noise covariance matrix +Eigen::MatrixXd Q = Eigen::MatrixXd::Identity(dim_x, dim_x); +Q *= 0.01; + +// Define measurement noise covariance matrix +Eigen::MatrixXd R = Eigen::MatrixXd::Identity(dim_y, dim_y); +R *= 1.0; + +// Initialize Kalman filter +autoware::kalman_filter::KalmanFilter kf; +kf.init(x0, P0); +``` + +- Predict step + +```cpp +const Eigen::MatrixXd x_next = A * x0; +kf.predict(x_next, A, Q); +``` + +- Update step + +```cpp +// Measured value +Eigen::MatrixXd y = Eigen::MatrixXd::Zero(dim_y, 1); +kf.update(y, C, R); +``` + +- Get the current estimated state and covariance matrix + +```cpp +Eigen::MatrixXd x_curr = kf.getX(); +Eigen::MatrixXd P_curr = kf.getP(); +``` + +## Assumptions / Known limits + +- Delay Step Check: Ensure that the `delay_step` provided during the update does not exceed the maximum delay steps set during initialization. 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 new file mode 100644 index 00000000..74db04f6 --- /dev/null +++ b/common/autoware_kalman_filter/include/autoware/kalman_filter/kalman_filter.hpp @@ -0,0 +1,214 @@ +// 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 new file mode 100644 index 00000000..80375b75 --- /dev/null +++ b/common/autoware_kalman_filter/include/autoware/kalman_filter/time_delay_kalman_filter.hpp @@ -0,0 +1,89 @@ +// 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 new file mode 100644 index 00000000..d43810a0 --- /dev/null +++ b/common/autoware_kalman_filter/package.xml @@ -0,0 +1,28 @@ + + + + autoware_kalman_filter + 0.0.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 new file mode 100644 index 00000000..bbd96367 --- /dev/null +++ b/common/autoware_kalman_filter/src/kalman_filter.cpp @@ -0,0 +1,161 @@ +// 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 new file mode 100644 index 00000000..4d1dd8f3 --- /dev/null +++ b/common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp @@ -0,0 +1,109 @@ +// 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 new file mode 100644 index 00000000..34e23ef9 --- /dev/null +++ b/common/autoware_kalman_filter/test/test_kalman_filter.cpp @@ -0,0 +1,96 @@ +// 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 new file mode 100644 index 00000000..50c22fae --- /dev/null +++ b/common/autoware_kalman_filter/test/test_time_delay_kalman_filter.cpp @@ -0,0 +1,123 @@ +// 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); +}