├── .gitignore
├── src
    ├── kalman_filter.cpp
    ├── test.cpp
    └── extend_kalman.cpp
├── CMakeLists.txt
├── README.md
└── include
    └── proj_kalman
        ├── kalman_filter.hpp
        ├── kalman_interface.hpp
        └── extend_kalman.hpp


/.gitignore:
--------------------------------------------------------------------------------
1 | .vscode
2 | build


--------------------------------------------------------------------------------
/src/kalman_filter.cpp:
--------------------------------------------------------------------------------
 1 | #include "proj_kalman/kalman_filter.hpp"
 2 | 
 3 | #include <iostream>
 4 | 
 5 | namespace proj_kalman {
 6 | void KalmanFilter::init(Eigen::VectorXd &x_k) {
 7 |     this->x_p_k = x_k;
 8 |     this->x_l_k = x_k;
 9 |     this->P = Eigen::MatrixXd::Zero(dim_x, dim_x);
10 | }
11 | 
12 | Eigen::VectorXd KalmanFilter::predict(Eigen::VectorXd &u, double t) {
13 |     double delta_t = t;
14 |     for (int i = 0; i < dim_x/2; i++) {
15 |         A(i, dim_x/2 + i) = delta_t;
16 |     }
17 |     x_p_k = A * x_l_k + B * u;
18 |     P = A * P * A.transpose() + Q;
19 |     return x_p_k;
20 | }
21 | 
22 | Eigen::VectorXd KalmanFilter::update(Eigen::VectorXd &z_k) {
23 |     for (int i = 0; i < dim_z; i++) {
24 |         H(i,i) = 1;
25 |     }
26 |     K = P * H.transpose() * (H * P * H.transpose() + R).inverse();
27 |     x_k = x_p_k + K * (z_k - H * x_p_k);
28 |     P = P - K * H * P;
29 |     x_l_k = x_k;
30 |     return x_k;
31 | }
32 | 
33 | }
34 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.8)
 2 | project(proj_kalman)
 3 | 
 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 5 |   add_compile_options(-Wall -Wextra -Wpedantic)
 6 | endif()
 7 | 
 8 | # include
 9 | include_directories(include)
10 | # include Eigen3 ( your path )
11 | include_directories("E:\\study\\c-tools\\eigen-3.2.10")
12 | 
13 | add_library(${PROJECT_NAME} SHARED
14 |     "src/kalman_filter.cpp"
15 |     "src/extend_kalman.cpp"
16 | )
17 | set(dependencies
18 |   Eigen3
19 |   eigen3_cmake_module
20 | )
21 | 
22 | add_executable(runtest "src/test.cpp")
23 | target_link_libraries(runtest ${PROJECT_NAME})
24 | 
25 | #install include directories
26 | install(DIRECTORY include/
27 |   DESTINATION include
28 | )
29 | 
30 | #install libraries
31 | install(TARGETS ${PROJECT_NAME}
32 |   EXPORT ${PROJECT_NAME}
33 |   LIBRARY DESTINATION lib
34 |   ARCHIVE DESTINATION lib
35 |   RUNTIME DESTINATION bin
36 |   INCLUDES DESTINATION include
37 | )
38 | 


--------------------------------------------------------------------------------
/src/test.cpp:
--------------------------------------------------------------------------------
 1 | #include "proj_kalman/kalman_filter.hpp"
 2 | 
 3 | int main() 
 4 | {
 5 |     double t = 1;
 6 |     Eigen::MatrixXd Q(4,4);
 7 |     Q << 0.1, 0, 0, 0,
 8 |         0, 0.1, 0, 0,
 9 |         0, 0, 0.1, 0,
10 |         0, 0, 0, 0.1;
11 |     Eigen::MatrixXd R(2,2);
12 |     R << 0.1, 0,
13 |         0, 0.1;
14 | 
15 |     // without control
16 |     Eigen::MatrixXd B = Eigen::MatrixXd::Zero(4,1);
17 |     Eigen::VectorXd u = Eigen::VectorXd::Zero(1);
18 | 
19 |     // // with control, acc = 1
20 |     // Eigen::MatrixXd B(4,1);
21 |     // B << 0.5 * pow(t, 2), 0.5 * pow(t, 2), t, t;
22 |     // Eigen::VectorXd u = Eigen::VectorXd::Zero(1);
23 |     // u(0,0) = 1;
24 |     auto kf = std::make_shared<proj_kalman::KalmanFilter>(4, 2, 0, Q, R, B);
25 |     Eigen::VectorXd x_0 = Eigen::VectorXd::Zero(4);
26 |     kf->init(x_0);
27 |     int iter = 1;
28 |     Eigen::VectorXd z_k = Eigen::VectorXd::Zero(2);
29 |     for (int i = 0; i < iter; i++) {
30 |         Eigen::VectorXd x_p_k = kf->predict(u, t);
31 |         z_k << 1, 1;
32 |         Eigen::VectorXd x_k = kf->update(z_k);
33 |     }
34 | 
35 |     return 0;
36 | }


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # 卡尔曼滤波器C++
 2 | 
 3 | ## 使用方法
 4 | 
 5 | ​		首先确定状态维度与测量量维度，以4维度$(x，y，vx，vy)$为状态量例子，以2维度$(x,y)$为测量量例子
 6 | 
 7 | ​		然后创建协方差矩阵Q与R，这两个矩阵是调参的主要矩阵，优劣很大程度取决于QR参数的选取。
 8 | 
 9 | ```c++
10 | 	Eigen::MatrixXd Q(4, 4); Eigen::MatrixXd R(2, 2);
11 | 	Q << 0.1, 0, 0, 0,
12 |      	0, 0.1, 0, 0,
13 |      	0, 0, 0.1, 0,
14 |      	0, 0, 0, 0.1;
15 | 	R << 0.1, 0,
16 |      	0, 0.1;
17 | ```
18 | 
19 | ​		再然后创建控制量矩阵B，如果没有控制，让控制向量u为0向量即可
20 | 
21 | ```c++
22 |     Eigen::MatrixXd B = Eigen::MatrixXd::Zero(4,1);
23 |     Eigen::VectorXd u = Eigen::VectorXd::Zero(1);
24 | ```
25 | 
26 | ​		如果是有控制量，假如是匀加速或者匀减速运动模型，为了方便，假设x y方向加速度一致，即u就为加速度量a；如果不一致u就是2x1的向量，x方向加速度与y方向加速度
27 | 
28 | ```c++
29 |     Eigen::MatrixXd B(4,1);
30 |     B << 0.5 * pow(t, 2), 0.5 * pow(t, 2), t, t;
31 |     Eigen::VectorXd u = Eigen::VectorXd::Zero(1);
32 |     u(0,0) = 1;
33 | ```
34 | 
35 | ​		构造卡尔曼类，并初始化
36 | 
37 | ```c++
38 |     auto kf = std::make_shared<proj_kalman::KalmanFilter>(4, 2, 0, Q, R, B);
39 |     Eigen::VectorXd x_0 = Eigen::VectorXd::Zero(4);
40 |     kf->init(x_0);
41 | ```
42 | 
43 | ​		在循环中不断进行预测与更新
44 | 
45 | ```c++
46 |     for (int i = 0; i < iter; i++) {
47 |         Eigen::VectorXd x_p_k = kf->predict(u, t);
48 |         Eigen::VectorXd x_k = kf->update(z_k);
49 |     }
50 | ```
51 | 
52 | 


--------------------------------------------------------------------------------
/include/proj_kalman/kalman_filter.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef PROJ_KALMAN_FILTER_HPP_
 2 | #define PROJ_KALMAN_FILTER_HPP_
 3 | 
 4 | #include "proj_kalman/kalman_interface.hpp"
 5 | 
 6 | namespace proj_kalman {
 7 | class KalmanFilter : public KalmanInterface {
 8 |     public:
 9 | 
10 | 	/**
11 | 	* @brief Construction of Kalman filter class
12 | 	* 
13 | 	* @param dim_x dimension of state(x_k)
14 | 	* @param dim_z dimension of measurement(z_k)
15 | 	* @param dim_u dimension of control(u_k)
16 | 	*/
17 |         KalmanFilter(int dim_x, int dim_z, int dim_u) 
18 |             : KalmanInterface(dim_x, dim_z, dim_u) {
19 |             if (dim_u > 0) 
20 |                 B = Eigen::MatrixXd::Zero(dim_x, dim_u);
21 |             else
22 |                 B = Eigen::MatrixXd::Zero(dim_x, dim_x);
23 |             	H = Eigen::MatrixXd::Zero(dim_z, dim_x);
24 |         }
25 | 
26 |         /**
27 | 	* @brief Construction of Kalman filter interface class
28 | 	* 
29 | 	* @param dim_x dimension of state(x_k)
30 | 	* @param dim_z dimension of measurement(z_k)
31 | 	* @param dim_u dimension of control(u_k)
32 |         * @param Q process error covariance matrix
33 |         * @param R measurement error covariance matrix
34 | 	*/
35 |         KalmanFilter(int dim_x, int dim_z, int dim_u, const Eigen::MatrixXd Q, 
36 |             const Eigen::MatrixXd R, const Eigen::MatrixXd B) : KalmanFilter(dim_x, dim_z, dim_u) {
37 |             this->Q = Q;
38 |             this->R = R;
39 |             this->B = B;
40 |         }
41 | 
42 | 	/**
43 | 	* @brief initialization of kalman filter(override)
44 | 	*
45 | 	* @param x_k initialization of state
46 | 	*/
47 |         void init(Eigen::VectorXd &x_k) override;
48 | 
49 |         /**
50 | 	* @brief prediction of kalman filter(override)
51 | 	*
52 | 	* @param u controlling quantity
53 |         * @param t step size
54 | 	*/
55 |         Eigen::VectorXd predict(Eigen::VectorXd &u, double t) override;
56 | 
57 |         /**
58 | 	* @brief update of kalman filter(override)
59 | 	*
60 | 	* @param z_k measurement quantity
61 | 	*/      
62 |         Eigen::VectorXd update(Eigen::VectorXd &z_k) override;
63 | 
64 |     public:
65 |         Eigen::MatrixXd B;  //  control quantity transfer matrix
66 |         Eigen::MatrixXd H;  //  dimensional transformation matrix
67 | };
68 | }
69 | 
70 | #endif // PROJ_KALMAN_FILTER_HPP_
71 | 


--------------------------------------------------------------------------------
/include/proj_kalman/kalman_interface.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef PROJ_KALMAN_INTERFACE_HPP_
 2 | #define PROJ_KALMAN_INTERFACE_HPP_
 3 | 
 4 | #include <cmath>
 5 | #include <string>
 6 | #include <iostream>
 7 | #include <memory>
 8 | #include <Eigen/Dense>
 9 | 
10 | namespace proj_kalman {
11 | class KalmanInterface {
12 |     public:
13 | 	/**
14 | 	* @brief Construction of Kalman filter interface class
15 | 	* 
16 | 	* @param dim_x dimension of state(x_k)
17 | 	* @param dim_z dimension of measurement(z_k)
18 | 	* @param dim_u dimension of control(u_k)
19 | 	*/
20 |         KalmanInterface(int dim_x, int dim_z, int dim_u): 
21 |             dim_x(dim_x), dim_z(dim_z), dim_u(dim_u) {
22 |             x_p_k = Eigen::VectorXd::Zero(dim_x, 1);
23 |             x_l_k = Eigen::VectorXd::Zero(dim_x, 1);
24 |             x_k = Eigen::VectorXd::Zero(dim_x, 1);
25 |             z_k = Eigen::VectorXd::Zero(dim_z, 1);
26 |             A = Eigen::MatrixXd::Identity(dim_x, dim_x);
27 | 	    K = Eigen::MatrixXd::Zero(dim_x, dim_z);
28 | 	    Q = Eigen::MatrixXd::Zero(dim_x, dim_x);
29 | 	    P = Eigen::MatrixXd::Zero(dim_x, dim_x);
30 | 	    R = Eigen::MatrixXd::Zero(dim_z, dim_z);
31 |     }
32 | 	/**
33 | 	* @brief initialization of kalman filter
34 | 	*
35 | 	* @param x_k initialization of state
36 | 	*/
37 |         virtual void init(Eigen::VectorXd &x_k) = 0;
38 | 
39 |         /**
40 | 	* @brief prediction of kalman filter
41 | 	*
42 | 	* @param u controlling quantity
43 |         * @param t step size
44 | 	*/
45 |         virtual Eigen::VectorXd predict(Eigen::VectorXd &u, double t) = 0;
46 | 
47 |         /**
48 | 	* @brief update of kalman filter
49 | 	*
50 | 	* @param z_k measurement quantity
51 | 	*/        
52 |         virtual Eigen::VectorXd update(Eigen::VectorXd &z_k) = 0;
53 | 
54 |     public:
55 | 
56 | 	int dim_x, dim_z, dim_u;			
57 | 
58 | 	Eigen::VectorXd x_p_k;  // predicted state quantity
59 | 	Eigen::VectorXd	x_k;    // updated state quantity
60 | 	Eigen::VectorXd	x_l_k;  // last moment state
61 | 	Eigen::VectorXd	z_k;    // measurement quantity
62 | 	Eigen::VectorXd	u;  // controlling quantity
63 | 
64 | 	Eigen::MatrixXd A;	// state-transition matrix
65 | 	Eigen::MatrixXd	P;  // state error covariance matrix
66 | 	Eigen::MatrixXd	K;  // gain matrix
67 | 	Eigen::MatrixXd	Q;  // process error covariance matrix
68 | 	Eigen::MatrixXd R;  // measurement error covariance matrix
69 |     };
70 | }
71 | 
72 | #endif // PROJ_KALMAN_INTERFACE_HPP_
73 | 


--------------------------------------------------------------------------------
/include/proj_kalman/extend_kalman.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef PROJ_EXTEND_KALMAN_HPP_
 2 | #define PROJ_EXTEND_KALMAN_HPP_
 3 | 
 4 | #include "proj_kalman/kalman_interface.hpp"
 5 | 
 6 | namespace proj_kalman {
 7 | 
 8 | const double DEC = M_PI / 180;
 9 | 
10 | class ExtendKalman : public KalmanInterface {
11 |     public:
12 | 
13 | 	/**
14 | 	* @brief Construction of Extend Kalman filter class
15 | 	* 
16 | 	* @param dim_x dimension of state(x_k)
17 | 	* @param dim_z dimension of measurement(z_k)
18 | 	* @param dim_u dimension of control(u_k)
19 | 	*/
20 |         ExtendKalman(int dim_x, int dim_z, int dim_u) 
21 |             : KalmanInterface(dim_x, dim_z, dim_u) {
22 |             H = Eigen::MatrixXd::Zero(dim_z, dim_x);
23 |         }
24 | 
25 |         /**
26 | 	* @brief Construction of Extend Kalman filter interface class
27 | 	* 
28 | 	* @param dim_x dimension of state(x_k)
29 | 	* @param dim_z dimension of measurement(z_k)
30 | 	* @param dim_u dimension of control(u_k)
31 |         * @param Q process error covariance matrix
32 |         * @param R measurement error covariance matrix
33 | 	*/
34 |         ExtendKalman(int dim_x, int dim_z, int dim_u, const Eigen::MatrixXd Q, 
35 |             const Eigen::MatrixXd R) : ExtendKalman(dim_x, dim_z, dim_u) {
36 |             this->Q = Q;
37 |             this->R = R;
38 |         }
39 | 
40 | 	/**
41 | 	* @brief initialization of extend kalman filter(override)
42 | 	*
43 | 	* @param x_k initialization of state
44 | 	*/
45 |         void init(Eigen::VectorXd &x_k) override;
46 | 
47 |         /**
48 | 	* @brief prediction of extend kalman filter(override)
49 | 	*
50 | 	* @param u controlling quantity
51 |         * @param t step size
52 | 	*/
53 |         Eigen::VectorXd predict(Eigen::VectorXd &u, double t) override;
54 | 
55 |         /**
56 | 	* @brief update of extend kalman filter(override)
57 | 	*
58 | 	* @param z_k measurement quantity
59 | 	*/      
60 |         Eigen::VectorXd update(Eigen::VectorXd &z_k) override;
61 | 
62 |         // The following functions are for reference only
63 |         Eigen::MatrixXd ctrv(Eigen::VectorXd &x_l_k, Eigen::VectorXd &u, double t);
64 |         Eigen::MatrixXd df_ctrv(Eigen::VectorXd &x_l_k, Eigen::VectorXd &u, double t);
65 |         Eigen::MatrixXd se_df_ctrv(Eigen::VectorXd &x_l_k, Eigen::VectorXd &u, double t);
66 |         Eigen::MatrixXd ctrv_sensor(Eigen::VectorXd &x_p_k);
67 |         Eigen::MatrixXd df_ctrv_sensor(Eigen::VectorXd &x_p_k);
68 |         Eigen::MatrixXd se_df_ctrv_sensor(Eigen::VectorXd &x_p_k);
69 | 
70 |     public:
71 |         Eigen::MatrixXd B;  //  control quantity transfer matrix
72 |         Eigen::MatrixXd H;  //  dimensional transformation matrix
73 | };
74 | }
75 | 
76 | #endif // PROJ_EXTEND_KALMAN_HPP_
77 | 


--------------------------------------------------------------------------------
/src/extend_kalman.cpp:
--------------------------------------------------------------------------------
  1 | #include "proj_kalman/extend_kalman.hpp"
  2 | 
  3 | namespace proj_kalman {
  4 | void ExtendKalman::init(Eigen::VectorXd &x_k) {
  5 |     	this->x_p_k = x_k;
  6 |     	this->x_l_k = x_k;
  7 |     	this->P = Eigen::MatrixXd::Zero(dim_x, dim_x);
  8 | }
  9 | 
 10 | Eigen::VectorXd ExtendKalman::predict(Eigen::VectorXd &u, double t) {
 11 |    	A = df_ctrv(x_l_k, u, t);
 12 |     	Eigen::MatrixXd W = se_df_ctrv(x_l_k, u, t);
 13 |    	x_p_k = ctrv(x_l_k, u, t);
 14 |     	P = A * P * A.transpose() + W * Q * W.transpose();
 15 |     	return x_p_k;
 16 | }
 17 | 
 18 | Eigen::VectorXd ExtendKalman::update(Eigen::VectorXd &z_k) {
 19 |     	H = df_ctrv_sensor(x_p_k);
 20 |     	Eigen::MatrixXd V = se_df_ctrv_sensor(x_p_k);
 21 |     	K = P * H.transpose() * (H * P * H.transpose() + V * R * V.transpose()).inverse();
 22 |     	x_k = x_p_k + K * (z_k - ctrv_sensor(x_p_k));
 23 |     	P = P - K * H * P;
 24 |     	x_l_k = x_k;
 25 |     	return x_k;
 26 | }
 27 | 
 28 | Eigen::MatrixXd ExtendKalman::ctrv(Eigen::VectorXd &x_l_k, Eigen::VectorXd &u, double t) {
 29 | 	double ctrv_delta_t = t;
 30 | 
 31 | 	B(0, 0) = cos(x_l_k(3, 0) * DEC) *
 32 | 				  pow(ctrv_delta_t, 2) / 2;
 33 | 	B(1, 0) = sin(x_l_k(3, 0) * DEC) *
 34 | 				  pow(ctrv_delta_t, 2) / 2;
 35 | 	B(2, 0) = ctrv_delta_t;
 36 | 	B(3, 1) = pow(ctrv_delta_t, 2) / 2;
 37 | 	B(4, 1) = ctrv_delta_t;
 38 | 	B(0, 1) = 0; B(1, 1) = 0; B(2, 1) = 0; B(3, 0) = 0; B(4, 0) = 0;
 39 | 
 40 | 	if (x_l_k(4, 0) != 0)
 41 | 	{
 42 | 		A(0, 0) = (1 + x_l_k(2, 0) * (sin(x_l_k(3, 0) * DEC + ctrv_delta_t * x_l_k(4, 0) * DEC) 
 43 |             - sin(x_l_k(3, 0) * DEC)) / (x_l_k(4, 0) * x_l_k(0, 0)));
 44 | 		A(1, 1) = (1 + x_l_k(2, 0) * (-cos(x_l_k(3, 0) * DEC + ctrv_delta_t * x_l_k(4, 0) * DEC) 
 45 |             + cos(x_l_k(3, 0) * DEC)) / (x_l_k(4, 0) * x_l_k(1, 0)));
 46 | 	}
 47 | 	else if (x_l_k(4, 0) == 0)
 48 | 	{
 49 | 		A(0, 0) = (1 + x_l_k(2, 0) * ctrv_delta_t * cos(x_l_k(3, 0) * DEC) / x_l_k(0, 0));
 50 | 		A(1, 1) = (1 + x_l_k(2, 0) * ctrv_delta_t * sin(x_l_k(3, 0) * DEC) / x_l_k(1, 0));
 51 | 	}
 52 | 	A(2, 2) = 1; A(3, 3) = 1; A(4, 4) = 1; A(3, 4) = ctrv_delta_t; A(0, 1) = 0;
 53 | 	A(0, 2) = 0; A(0, 3) = 0; A(0, 4) = 0; A(1, 0) = 0; A(1, 2) = 0; A(1, 3) = 0;
 54 | 	A(1, 4) = 0; A(2, 0) = 0; A(2, 1) = 0; A(2, 3) = 0; A(2, 4) = 0; A(3, 0) = 0;
 55 | 	A(3, 1) = 0; A(3, 2) = 0; A(4, 0) = 0; A(4, 1) = 0; A(4, 2) = 0; A(4, 3) = 0;
 56 | 
 57 | 	x_p_k = A * x_l_k + B * u;
 58 | 	return x_p_k;
 59 | }
 60 | 
 61 | Eigen::MatrixXd ExtendKalman::df_ctrv(Eigen::VectorXd &x_l_k, Eigen::VectorXd &u, double t)
 62 | {
 63 | 	(void) u;
 64 | 	double ctrv_delta_t = t;
 65 | 	Eigen::MatrixXd F_Matrix(5, 5);
 66 | 
 67 | 	if (x_l_k(4, 0) != 0) {
 68 | 		F_Matrix(0, 0) = 1;
 69 | 		F_Matrix(0, 1) = 0;
 70 | 		F_Matrix(0, 2) = (-sin(x_l_k(3, 0) * DEC) + sin(ctrv_delta_t * x_l_k(4, 0) * DEC + x_l_k(3, 0) * DEC)) / x_l_k(4, 0);
 71 | 		F_Matrix(0, 3) = x_l_k(2, 0) * (-cos(x_l_k(3, 0) * DEC) + cos(ctrv_delta_t * x_l_k(4, 0) * DEC + x_l_k(3, 0) * DEC)) / x_l_k(4, 0);
 72 | 		F_Matrix(0, 4) = ctrv_delta_t * x_l_k(2, 0) / x_l_k(4, 0) * cos(ctrv_delta_t * x_l_k(4, 0) * DEC + x_l_k(3, 0) * DEC) -
 73 | 							 x_l_k(2, 0) / pow(x_l_k(4, 0), 2) * (-sin(x_l_k(3, 0) * DEC) + sin(ctrv_delta_t * x_l_k(4, 0) * DEC + x_l_k(3, 0) * DEC));
 74 | 		F_Matrix(1, 0) = 0;
 75 | 		F_Matrix(1, 1) = 1;
 76 | 		F_Matrix(1, 3) = x_l_k(2, 0) * (-sin(x_l_k(3, 0) * DEC) + sin(ctrv_delta_t * x_l_k(4, 0) * DEC + x_l_k(3, 0) * DEC)) / x_l_k(4, 0);
 77 | 		F_Matrix(1, 2) = (cos(x_l_k(3, 0) * DEC) - cos(ctrv_delta_t * x_l_k(4, 0) * DEC + x_l_k(3, 0) * DEC)) / x_l_k(4, 0);
 78 | 		F_Matrix(1, 4) = ctrv_delta_t * x_l_k(2, 0) / x_l_k(4, 0) * sin(ctrv_delta_t * x_l_k(4, 0) * DEC + x_l_k(3, 0) * DEC) - x_l_k(2, 0) /
 79 | 						pow(x_l_k(4, 0), 2) * (cos(x_l_k(3, 0) * DEC) - cos(ctrv_delta_t * x_l_k(4, 0) * DEC + x_l_k(3, 0) * DEC));
 80 | 	}
 81 | 	else if (x_l_k(4, 0) == 0) {
 82 | 		F_Matrix(0, 0) = 1; F_Matrix(0, 1) = 0; F_Matrix(0, 4) = 0;
 83 | 		F_Matrix(0, 2) = ctrv_delta_t * cos(x_l_k(3, 0) * DEC);
 84 | 		F_Matrix(0, 3) = -ctrv_delta_t * x_l_k(2, 0) * sin(x_l_k(3, 0) * DEC);
 85 | 		F_Matrix(1, 0) = 0; F_Matrix(1, 1) = 1; F_Matrix(1, 4) = 0;
 86 | 		F_Matrix(1, 2) = ctrv_delta_t * sin(x_l_k(3, 0) * DEC);
 87 | 		F_Matrix(1, 3) = ctrv_delta_t * x_l_k(2, 0) * cos(x_l_k(3, 0) * DEC);
 88 | 	}
 89 | 	F_Matrix(2, 0) = 0; F_Matrix(2, 1) = 0; F_Matrix(2, 2) = 1; F_Matrix(2, 3) = 0;
 90 | 	F_Matrix(2, 4) = 0; F_Matrix(3, 0) = 0; F_Matrix(3, 1) = 0; F_Matrix(3, 2) = 0;
 91 | 	F_Matrix(3, 3) = 1; F_Matrix(3, 4) = ctrv_delta_t; F_Matrix(4, 0) = 0; F_Matrix(4, 1) = 0;
 92 | 	F_Matrix(4, 2) = 0; F_Matrix(4, 3) = 0; F_Matrix(4, 4) = 1;
 93 | 
 94 | 	return F_Matrix;
 95 | }
 96 | 
 97 | Eigen::MatrixXd ExtendKalman::se_df_ctrv(Eigen::VectorXd &x_l_k, Eigen::VectorXd &u, double t) {
 98 | 	(void) x_l_k;
 99 | 	(void) u;
100 | 	double ctrv_delta_t = t;
101 | 	(void) ctrv_delta_t;
102 | 	Eigen::MatrixXd Matrix(5, 5);
103 | 	Matrix(0, 0) = 1; Matrix(0, 1) = 0; Matrix(0, 2) = 0; Matrix(0, 3) = 0; Matrix(0, 4) = 0;
104 | 	Matrix(1, 0) = 0; Matrix(1, 1) = 1; Matrix(1, 2) = 0; Matrix(1, 3) = 0; Matrix(1, 4) = 0;
105 | 	Matrix(2, 0) = 0; Matrix(2, 1) = 0; Matrix(2, 2) = 1; Matrix(2, 3) = 0; Matrix(2, 4) = 0;
106 | 	Matrix(3, 0) = 0; Matrix(3, 1) = 0; Matrix(3, 2) = 0; Matrix(3, 3) = 1; Matrix(3, 4) = 0;
107 | 	Matrix(4, 0) = 0; Matrix(4, 1) = 0; Matrix(4, 2) = 0; Matrix(4, 3) = 0; Matrix(4, 4) = 1;
108 | 
109 | 	return Matrix;
110 | }
111 | 
112 | Eigen::MatrixXd ExtendKalman::ctrv_sensor(Eigen::VectorXd &x_p_k) {
113 | 	Eigen::MatrixXd Matrix(2, 5);
114 | 	Matrix(0, 0) = 1; Matrix(0, 1) = 0; Matrix(0, 2) = 0; Matrix(0, 3) = 0;
115 | 	Matrix(0, 4) = 0; Matrix(1, 0) = 0; Matrix(1, 1) = 1; Matrix(1, 2) = 0;
116 | 	Matrix(1, 3) = 0; Matrix(1, 4) = 0; 
117 |     
118 |     	Matrix = Matrix * x_p_k;
119 | 
120 | 	return Matrix;
121 | }
122 | 
123 | Eigen::MatrixXd ExtendKalman::df_ctrv_sensor(Eigen::VectorXd &x_p_k) {
124 | 	(void) x_p_k;
125 | 	Eigen::MatrixXd Matrix(2, 5);
126 | 	Matrix(0, 0) = 1; Matrix(0, 1) = 0; Matrix(0, 2) = 0; Matrix(0, 3) = 0;
127 | 	Matrix(0, 4) = 0; Matrix(1, 0) = 0; Matrix(1, 1) = 1; Matrix(1, 2) = 0;
128 | 	Matrix(1, 3) = 0; Matrix(1, 4) = 0;
129 | 
130 | 	return Matrix;
131 | }
132 | 
133 | Eigen::MatrixXd ExtendKalman::se_df_ctrv_sensor(Eigen::VectorXd &x_p_k) {
134 | 	(void) x_p_k;
135 | 	Eigen::MatrixXd Matrix(2, 2);
136 | 	Matrix(0, 0) = 1; Matrix(0, 1) = 0;
137 | 	Matrix(1, 0) = 0; Matrix(1, 1) = 1;
138 | 	return Matrix;
139 | }
140 | 
141 | }
142 | 


--------------------------------------------------------------------------------