无迹卡尔曼滤波在目标跟踪中的作用（二）

在上一节的内容中，我们介绍了UKF中最重要的内容—无迹变换UT，今天我们将具体介绍UKF是如何实现的。
好了，话不多说，开整！！！

UKF算法的实现

我们知道，我们可以使用状态方程和观测方程来对系统进行描述，那么一个非线性系统可以用以下的方程进行描述：
$$\{\begin{array}{l}X\left(k+1\right)=f\left(x\left(k\right),V\left(k\right)\right)\\ Z\left(k\right)=h\left(x\left(k\right),W\left(k\right)\right)\end{array}$$
其中的

• $f$为非线性状态方程函数
• $V(k)$为过程噪声，其协方差矩阵为Q
• $h$为非线性观测方程函数
• $W(k)$为量测噪声，协方差矩阵为R

在描述完非线性系统后，对不同时刻的数据进行滤波，步骤如下：

• 1、首先获得一组Sigma点集（采样点集）与权值，计算如下式：
  
  详细介绍请见：
  UKF在目标跟踪中的应用（一）
  也即：
  $$X^{(i)}(k|k)=[\hat{X}(k|k)\hat{X}(k|k)+\sqrt{(n+\lambda )P(k|k)}\hat{X}(k|k)-\sqrt{(n+\lambda )P(k|k)}]$$

• 2、然后计算2n+1个Sigma点的一步预测值,i=1，2……2n+1；
  $$X^{(i)}(k+1|k)=f[k,X^{(i)}(k|k)]$$

• 3、计算系统状态量和协方差矩阵的一步预测值

$$\hat{X}(k+1|k)=\sum _{i=0}^{2n}{\omega }^{(i)}{X}^{(i)}(k+1|k)$$
$$P(k+1|k)=\sum _{k=0}^{2n}{\omega }^{(k)}[\hat{X}(k+1|k)-X^{(k)}(k+1|k)][\hat{X}(k+1|k)-X^{(n)}(k+1|k){]}^T+Q$$

此处就与Kalman明显不同：
在Kalman滤波中，只需将上一时刻的值带入状态方程计算一次即可得到预测值，UKF是将一组采样点值进行预测，然后加权，得到状态的预测值

• 4、再根据一步预测值，使用UT变换，产生新的Sigma点集，
  $$X^{(i)}(k+1|k)=[\hat{X}(k+1|k)\hat{X}(k+1|k)+\sqrt{(n+\lambda )P(k+1|k)}\hat{X}(k+1|k)-\sqrt{(n+\lambda )P(k+1|k)}]$$

• 5、将步骤4中新产生的Sigma点集带入观测方程，得到观测的预测值，i=1，2，……2n+1；
  $$Z^{(i)}\left(k+1|k\right)=h[X^{(i)}(k+1|k)]$$

• 6、通过步骤5中得到的Sigma点集的观测预测值，加权求和得到系统预测的均值和方差，
  $$\overline{Z}(k+1|k)=\sum _{i=0}^{2n}{\omega }^{(i)}{Z}^{(i)}(k+1|k)$$
  $$P_{z_k{z}_k}=\sum _{i=0}^{2n}{\omega }^{(i)}[Z^{(i)}(k+1|k)-\overline{Z}(k+1|k)][Z^{(i)}(k+1|k)-\overline{Z}(k+1|k){]}^{\mathrm{T}}+R$$
  $$P_{x_k{z}_k}=\sum _{i=0}^{2\pi }{\omega }^{(i)}[X^{(i)}(k+1|k)-\overline{Z}(k+1|k)][Z^{(i)}(k+1|k)-\overline{Z}(k+1|k){]}^T$$

• 7、计算滤波增益K
  $$K(k+1)=P_{x_k{z}_k}{P}_{z_k{z}_k}^{-1}$$

• 8、状态更新、协方差更新：
  $$\hat{X}\left(k+1|k+1\right)=\hat{X}\left(k+1|k\right)+K\left(k+1\right)\left[Z\left(k+1\right)-\hat{Z}\left(k+1|k\right)\right]$$
  $$P(k+1|k+1)=P(k+1|k)-K(k+1)P_{z_k{z}_k}{K}^{⊺}(k+1)$$

经过上述的步骤，即可完成一次滤波，然后不断循环进行即可，如果用一张图表示UKF滤波过程，可以如下图所示：

下次将对其进行MATLAB仿真验证。

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