容积卡尔曼滤波CKF—目标跟踪中的应用(算法部分)

原创不易，路过的各位大佬请点个赞

作者:823618313@qq.com
备注：
容积卡尔曼滤波算法；CKF；Cubature Kalman Filter
两种CKF算法：加性噪声CKF和非加性噪声CKF
本博客主要讲解“加性噪声条件下的容积卡尔曼滤波算法”推导结果
matlab实现;
目标跟踪仿真
Case: 二维目标跟踪情况和三维目标跟踪情况
代码下载地址如下（分别为二维情形和三维情形）

容积卡尔曼滤波思考：

无迹卡尔曼滤波UKF—目标跟踪中的应用

• 容积卡尔曼滤波CKF—目标跟踪中的应用(算法部分)
• • 1、带加性噪声的容积卡尔曼滤波算法CKF
  • • 1.1 问题描述（离散时间非线性系统描述）

1、带加性噪声的容积卡尔曼滤波算法CKF

1.1 问题描述（离散时间非线性系统描述）

考虑带加性噪声的一般非线性系统模型，
x k = f ( x k − 1 ) + w k − 1 z k = h ( x k ) + v k (1) x_k=f(x_{k-1}) +w_{k-1} \\ z_k=h(x_k)+v_k \tag{1} xk​=f(xk−1​)+wk−1​zk​=h(xk​)+vk​(1)
其中 x k x_k xk​为 k k k时刻的目标状态向量。 z k z_k zk​为 k k k时刻量测向量（传感器数据）。这里不考虑控制器 u k u_k uk​。 w k {w_k} wk​和 v k {v_k} vk​分别是过程噪声序列和量测噪声序列，并假设 w k w_k wk​和 v k v_k vk​为零均值高斯白噪声，其方差分别为 Q k Q_k Qk​和 R k R_k Rk​的高斯白噪声，即 w k ∼ ( 0 , Q k ) w_k\sim(0,Q_k) wk​∼(0,Qk​), v k ∼ ( 0 , R k ) v_k\sim(0,R_k) vk​∼(0,Rk​)，且满足如下关系(线性高斯假设)为：
E [ w i v j ′ ] = 0 E [ w i w j ′ ] = 0 i ≠ j E [ v i v j ′ ] = 0 i ≠ j \begin{aligned} E[w_iv_j'] &=0\\ E[w_iw_j'] &=0\quad i\neq j \\ E[v_iv_j'] &=0\quad i\neq j \end{aligned} E[wi​vj′​]E[wi​wj′​]E[vi​vj′​]​=0=0i​=j=0i​=j​