卡尔曼滤波在目标跟踪中的应用（4）

在前一节内容中，我们学习了二维匀加速运动目标的卡尔曼滤波问题，同时利用MATLAB进行了仿真验证，今天我们继续往下扩展一个维度，学习三维空间下的卡尔曼滤波问题。
话不多说，开整！！！

状态方程

既然目标是在三维空间中做匀加速运动，那么自然其在x、y、z三个方向上均会产生位移和速度，并且每个方向都有着加速度，因此，根据运动学公式，其各个方向的位置、速度、加速度可由下式得出：
$$x_k=x_{k-1}+{\dot{x}}_{k-1}\Delta t+\frac{1}{2}{\ddot{x}}_{k-1}\Delta t^2(1)$$
$${\dot{x}}_k={\dot{x}}_{k-1}+{\ddot{x}}_{k-1}\Delta t(2)$$
$${\ddot{x}}_k={\ddot{x}}_{k-1}(3)$$
$$y_k=y_{k-1}+{\dot{y}}_{k-1}\Delta t+\frac{1}{2}{\ddot{y}}_{k-1}\Delta t^2(4)$$
$${\dot{y}}_k={\dot{y}}_{k-1}+{\ddot{y}}_{k-1}\Delta t(5)$$
$${\ddot{y}}_k={\ddot{y}}_{k-1}(6)$$
$$z_k=z_{k-1}+{\dot{z}}_{k-1}\Delta t+\frac{1}{2}{\ddot{z}}_{k-1}\Delta t^2(7)$$
$${\dot{z}}_k={\dot{z}}_{k-1}+{\ddot{z}}_{k-1}\Delta t(8)$$
$${\ddot{z}}_k={\ddot{z}}_{k-1}(9)$$

其中：
$x_k$:目标在k时刻的x方向的位置；
${\dot{x}}_k$:目标在k时刻的x方向的速度；
${\ddot{x}}_k$:目标在k时刻的x方向的加速度;
$x_{k-1}$:目标在k-1时刻的x方向的位置；
${\dot{x}}_{k-1}$:目标在k-1时刻的x方向的速度；
${\ddot{x}}_{k-1}$:目标在k-1时刻的x方向的加速度

y、z方向的各参数意义同上，不再叙述。
那么此时的目标状态可以表示为：
$$X_k=[x_k{\dot{x}}_k{\ddot{x}}_k{y}_k{\dot{y}}_k{\ddot{y}}_k{z}_k{\dot{z}}_k{\ddot{z}}_k]$$

考虑目标在运动过程中的过程噪声$V_k$的影响，将式（1）~（9）写为矩阵相乘形式，则可以表示下式：
X k = [ 1 Δ t 1 2 Δ t 2 0 0 0 0 0 0 0 1 Δ t 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 Δ t 1 2 Δ t 2 0 0 0 0 0 0 0 1 Δ t 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 Δ t 1 2 Δ t 2 0 0 0 0 0 0 0 1 Δ t 0 0 0 0 0 0 0 0 1 ] X k − 1 + V k = F X k − 1 + V k X_{k} = \begin{bmatrix}1&\Delta t&\frac{1}{2}\Delta t^2&0&0&0&0&0&0\\[0.3em]0&1&\Delta t&0&0&0&0&0&0\\[0.3em]0&0&1&0&0&0&0&0&0\\[0.3em]0&0&0&1&\Delta t&\frac{1}{2}\Delta t^2&0&0&0\\[0.3em]0&0&0&0&1&\Delta t&0&0&0\\[0.3em]0&0&0&0&0&1&0&0&0 \\ 0&0&0&0&0&0&1&\Delta t&\frac{1}{2}\Delta t^2\\0&0&0&0&0&0&0&1&\Delta t\\0&0&0&0&0&0&0&0&1\end{bmatrix}X_{k-1} + V_k = FX_{k-1} +V_k Xk​= ​100000000​Δt10000000​21​Δt2Δt1000000​000100000​000Δt10000​00021​Δt2Δt1000​000000100​000000Δt10​00000021​Δt2Δt1​ ​Xk−1​+Vk​=FXk−1​+Vk​
至此，目标的状态方程便已介绍完成，其中F称为状态转移矩阵，下面介绍观测方程。

观测方程

同样，观测方程仍然是雷达测量过程的假设，那么在三维空间的背景下，此时的测量值$Z_k$就是三个方向的位置信息也即：
$$Z_k=[z_x{z}_y{z}_z]$$
量测值与真实值的关系即可以通过观测方程进行描述，考虑量测过程中的不确定性即量测噪声$R_k$如下式：
Z k = [ 1 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 1 0 0 ] X k + W k = H X k + W k \begin{aligned} \\ Z_k&& =\begin{bmatrix}1&0&0&0&0&0&0&0&0\\ 0&0&0&1&0&0&0&0&0\\0&0&0&0&0&0&1&0&0\end{bmatrix}X_k+W_k=H X_k+W_k \end{aligned} Zk​​​= ​100​000​000​010​000​000​001​000​000​ ​Xk​+Wk​=HXk​+Wk​​

至此，量测方程也介绍完毕，下面进行仿真测试。

MATLAB仿真

在对二维匀加速运动目标建模完成后，即可对其进行MATLAB仿真，仿真背景如下：

• 采样点数：200
• 采样时间：0.1s
• 初始位置：[7 11 21]
• 初始速度：[10 20 15]
• 初始加速度：[0.5 1 0.75]
• 过程噪声方差：1e-6
• 量测噪声方差：100

下面对其进行仿真，其中：

> 观测误差 = 观测值 - 真实值 
>  滤波误差 = 滤波值 - 真实值

x方向的位置滤波效果对比

x方向的距离滤波误差对比

x方向的速度滤波效果对比

x方向的速度滤波误差对比

x方向的加速度滤波效果

其余两个方向的滤波效果不在此处进行展示，从效果中我们发现，随着跟踪时间的递进，滤波的效果明显越来越好。

上述内容即使今天的全部内容了，感谢大家的观看。
如果方便，辛苦大家点个赞和关注哦！
您的点赞或评论或关注是对我最大的肯定，谢谢大家！！！