背景

上文讨论的EKF方式，通过对系统方程或者观测方程进行泰勒展开后，仅保留其一阶近似项，这样的做法不可避免的会造成一些误差，如果系统的非线性程度不是很强的话，那还好说，误差可以忽略，强非线性系统的话，这种误差就必须要考虑了；另外，每次都要进行一次雅克比矩阵计算，很多计算平台做偏导计算是不太容易实现的。

基于上面所述的两个缺点，UKF则完全没有这两个问题，不过本质的思想都是一样的，UKF使用无迹变换(Unscented Transform,UT)来处理非线性方程的均值和协方差的传递问题。

UKF的核心思想是对非线性函数的概率密度分布进行近似，用一系列的确定样本来逼近状态的后验概率密度。UKF没有把高阶项忽略，因此UKF有较高的精度。

无迹变换

无迹变换就是在估计点附近进行采样，使用这些样本点表示高斯分布近似状态的概率密度函数。其步骤为：

1. 在原状态分布中，按照规则选取一定的采样点；
2. 将这些采样点带入非线性函数中，得到非线性函数值点集，通过这些点集求取变换后的均值和协方差。

设一个非线性变换$y=f(x)$，其中状态向量$x$为$n$维随机变量，并且已知其均值为$\overline{x}$，协方差为$P$。

上图所示，即为UT变换，下面说一下，如何通过UT变换得到$2n+1$个$Sigma$点$\chi$和相应的权重$\omega$来计算$y$的统计特征（上标表示为采样点的序列数，下标$m$为均值，下标$c$为协方差）。

第一步，计算当前时刻的$Sigma$点，
$$\{\begin{array}{l}{\chi }^0=\mu ,i=0\\ {\chi }^i=\mu +(\sqrt{(n+\lambda )P}{)}_i,i=1,\dots ,n\\ {\chi }^i=\mu -(\sqrt{(n+\lambda )P}{)}_i,i=n+1,\dots ,2n\end{array}$$

上式中，$n$为系统状态的维度，
$\lambda$为缩放因子，
$(\sqrt{(n+\lambda )P}{)}_i$表示矩阵$(n+\lambda )P$平方根的第$i$列（当$P=A^{T}A$时，$({\sqrt{P}}_i$取$A$的第$i$行；）。

第二步，计算下一时刻经非线性变换产生的预测采样点，
$$Y^i=f({\chi }^i)$$

第三步，计算对应的权重为
$$\{\begin{array}{l}{w}_m^0=\frac{\lambda }{n+\lambda }\\ w_m^0=\frac{\lambda }{n+\lambda }+(1-{\alpha }^2+\beta )\\ w_m^i=w_c^i=\frac{1}{2(n+\lambda )},i=1,\dots ,2n\end{array}$$

其中，缩放因子$\lambda =\alpha (n+k)-n$用来降低总的预测误差，$\alpha$决定了采样点的分布状态，一般取值范围为$10^{-4}\le \alpha \le 1$；至于$k$的选取则没有太多的限制，但是起码要保证$(n+\lambda )P$为半正定矩阵，一般都是选取$n+k=3$；参数$\beta \ge 0$，可以通过调节此项提高方差的精度，把高阶项的影响考虑进来，对于正态分布，选取$\beta =2$。

第四步，计算非线性变换后的均值和协方差，
$$\{\begin{array}{l}\bar{Y}=\sum _{i=0}^{2n}{w}_m^i{Y}^i\\ P_{YY}=\sum _{i=0}^{2n}{w}_c^i[Y^i-\bar{Y}][Y^i-\bar{Y}{]}^T\end{array}$$

UKF的实现

由具有高斯白噪声$W(k)$的随机变量$X$和具有高斯白噪声$V(k)$的观测变量$Z$构成的非线性系统，如下所示，
$$\{\begin{array}{l}X(k+1)=f(X(k),W(k))\\ Z(k)=h(X(k),V(k))\end{array}$$
式中，$f$为非线性状态方程函数，$h$为观测方程函数，$W(k)$的协方差矩阵为$Q$，$V(k)$的协方差矩阵为$R$。

具体的UKF计算步骤如下：

1. 计算当前时刻的$Sigma$点
   $$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)}]\text{\hspace{1em}(1)}$$
2. 预测当前时刻的$Sigma$点集的一步预测
   $$X^i(k+1|k)=f[k,X^i(k|k)]\text{\hspace{1em}(2)}$$
3. 计算一步预测的均值和协方差矩阵
   $$\{\begin{array}{l}\hat{X}(k+1|k)=\sum _{i=0}^{2n}{w}^i{X}^i(k+1|k)\\ P(k+1|k)=\sum _{i=0}^{2n}{w}^i[\hat{X}(k+1|k)-X^i(k+1|k)][\hat{X}(k+1|k)-X^i(k+1|k){]}^T+Q\end{array}\text{\hspace{1em}(3)}$$
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)}]\text{\hspace{1em}(4)}$$
5. 将$k+1$时刻的$Sigma$点集带入观测方程，得到预测的观测量，
   $$Z^i(k+1|k)=\sum _{i=0}^{2n}{w}^i{Z}^i(k+1|k)\text{\hspace{1em}(5)}$$
6. 由第五步得到$Sigma$点集的观测预测值，通过加权求和得到系统预测的均值以及协方差
   $$\{\begin{array}{l}\bar{Z}(k+1|k)=\sum _{i=0}^{2n}{w}^i{Z}^i(k+1|k)\\ P_{z_k{z}_k}=\sum _{i=0}^{2n}{w}^i[Z^i(k+1|k)-\bar{Z}(k+1|k)][Z^i(k+1|k)-\bar{Z}(k+1|k){]}^T+R\\ P_{x_k{z}_k}=\sum _{i=0}^{2n}{w}^i[X^i(k+1|k)-\bar{Z}(k+1|k)][X^i(k+1|k)-\bar{Z}(k+1|k){]}^T\end{array}\text{\hspace{1em}(6)}$$
7. 计算卡尔曼增益矩阵
   $$K(k+1)=P_{x_k{z}_k}{P}_{z_k{z}_k}^{-1}\text{\hspace{1em}(7)}$$
8. 最后，计算系统的状态更新和协方差更新

$$\{\begin{array}{l}\hat{X}(k+1|k+1)=\hat{X}(k+1|k)+K(k+1)[Z(k+1)-\hat{Z}(k+1|k)]\\ P(k+1|k+1)=P(k+1|k)-K(k+1)P_{z_k{z}_k}K(k+1{)}^T\end{array}\text{\hspace{1em}(8)}$$

总结

至此，卡尔曼的三种滤波方式都学完了，线性系统就卡尔曼，非线性不强的系统就使用扩展卡尔曼，强非线性系统无迹卡尔曼。

卡尔曼的原理就不说了，第一篇有很详细的解释。

扩展卡尔曼利用泰勒展开对非线性系统进行拟合，在对展开的系统舍去高阶项，所以不可避免的有一些精度损失，所以不适用于强非线性系统。

无迹卡尔曼的思路则与扩展卡尔曼不同，在估计点附近进行UT变换，使用获得的$Sigma$点集的均值和协方差与原统计特征进行匹配，再把$Sigma$点集进行非线性映射，以近似得到状态概率密度函数，这种思路本质其实是一种统计近似。