KF、EKF、IEKF、UKF卡尔曼滤波器

1. 概述

1.1 开篇

对于卡尔曼滤波的直观理解便是对所掌握数据的融合处理，假设手头有两个随机变量，知道其均值和方差，而且均值相同，那么我希望找到一个权重，使得把这两个随机变量组合起来，均值不变，方差减小，从线条上看当然变得平稳了一些，于是被搞工程的人成为滤波。

问题是权重如何找呢？简化一下问题，把这两个随机变量都视作服从正态分布的随机变量，这样通过计算可知，权重与它们的方差有关。

下一个问题是，往往手头只有一个滤波器，怎么办？可以给系统建模，根据历史信息计算出一个额外的数值来，为了简化问题，认为系统是线性的，那么输入数学模型给出的数值任然服从某个正态分布，均值和方差都可以从模型中导出。然后就可以开开心心地滤波了。

齐次要理解粒子滤波。接上，然而问题接踵而至，系统的模型往往都不是线性的啊！输入个正态分布的量，有的连解析式都写不出来，根本不知道输出的是个啥啊！于是想了个办法，我们不用均值和方差来表示随机变量，我们用数量居多的采样点来表示！！！把每个采样带进模型去计算，统统算完就得到了下一次的随机变量的采样点，当需要计算权重的时候再把方差给算出来。在计算力十分十分强大的仿真平台上，这方法还能说得过去。因为一般要用到的采样点很多，所以也被称作粒子云滤波。

现在可以理解无迹卡尔曼滤波了，在一般情况下，应用粒子滤波困难重重，因为那些粒子云经过多次迭代，很容易发生聚集，这样就无法精准的描述随机变量了，所以一般要择机向粒子云里稀疏的地方添加粒子，在太过稠密的地方减去粒子，不过谁能保证这么操作不会带来均值与方差的偏差？

此外为了达到精度要求，粒子的数量需要很大，这很难算的过来啊。此时相信你已经体会到了，如何更准更快的计算某个随机变量经过某个函数变成了怎样的随机变量才是卡尔曼滤波器的关键所在。无迹变换是尝试解决这个问题的一个方案，通过某种规则去采样，在粒子数量尽量小的情况下，去保证采样点云的均值和方差不变。不过我没做过无迹卡尔曼的仿真，对ut的理解不深入。

ref：无迹卡尔曼到底是什么东西？

1.2 问题定义

在时序运动系统中为了得到当前时刻自身的状态信息，一方面需要根据以往数据对当前时刻进行估计，同时需要使用传感器等测量工具对自身状态进行测量，进而调整估计模型使得估计更加准确。
$${\bar{x}}_t={\hat{x}}_t+{\mathcal{K}}_t({\dot{x}}_t-\mathcal{H}\cdot {\hat{x}}_t)$$
${\mathcal{K}}_t\in [0,1.0]$为卡尔曼增益，$\mathcal{H}$为测量转换矩阵。符号统一定义：

• 1）真实无偏差状态状态信息：$x_t$
• 2）根据以往数据得到的当前时刻预测结果：${\hat{x}}_t$
• 3）当前利用传感器等测量手段得到的测量结果：${\dot{x}}_t$
• 4）根据现有预测结果和传感器测量结果得到的最后估计结果：${\bar{x}}_t$

2. KF滤波器

2.1 KF下模型建立与推导

KF滤波器是线型滤波器，其理想状态转移方程（也可称为运动方程）可以描述为：
$$x_t=\mathcal{F}\cdot x_{t-1}+\mathcal{B}\cdot u_t+{\omega }_t$$
其中，$\mathcal{F}$代表状态转移矩阵，$\mathcal{B}$代表控制矩阵，$u_t$代表控制信号（如加速度、转向角度等变量），${\omega }_t$代表系统噪声扰动，服从高斯分布。而上述的过程是理想情况下对线型系统的建模，而实际中对于实际运动预测描述为：
$${\hat{x}}_t=\mathcal{F}\cdot {\bar{x}}_{t-1}+\mathcal{B}\cdot u_t$$
注意，上面的式子中运动状态为上一时刻的估计结果${\bar{x}}_{t-1}$，而不是上一时刻的预测结果${\hat{x}}_{t-1}$。在上述的过程中可以看到运动估计结果与真实无偏结果是存在偏差的，这里使用协方差去度量它们之间的差异：
$$\begin{array}{rl}{\hat{P}}_t& =E(\bar{e}{\bar{e}}^T)=E((x_t-{\hat{x}}_t)(x_t-{\hat{x}}_t{)}^T)\\ & =E((\mathcal{F}\cdot x_{t-1}+\mathcal{B}\cdot u_t+{\omega }_t-\mathcal{F}\cdot {\bar{x}}_{t-1}-\mathcal{B}\cdot u_t)(\mathcal{F}\cdot x_{t-1}+\mathcal{B}\cdot u_t+{\omega }_t-\mathcal{F}\cdot {\bar{x}}_{t-1}-\mathcal{B}\cdot u_t{)}^T)\\ & =E((\mathcal{F}(x_{t-1}-{\bar{x}}_{t-1})+{\omega }_t)(\mathcal{F}(x_{t-1}-{\bar{x}}_{t-1})+{\omega }_t{)}^T)\\ & =E(\mathcal{F}(x_{t-1}-{\bar{x}}_{t-1})(x_{t-1}-{\bar{x}}_{t-1}{)}^T{\mathcal{F}}^T+{\omega }_t{\omega }_t^T+\mathcal{F}(x_{t-1}-{\bar{x}}_{t-1}){\omega }_t^T+{\omega }_t(x_{t-1}-{\bar{x}}_{t-1}{)}^T{\mathcal{F}}^T)\\ & =\mathcal{F}E((x_{t-1}-{\bar{x}}_{t-1})(x_{t-1}-{\bar{x}}_{t-1}{)}^T){\mathcal{F}}^T+E({\omega }_t{\omega }_t^T)\\ & =\mathcal{F}E((x_{t-1}-{\bar{x}}_{t-1})(x_{t-1}-{\bar{x}}_{t-1}{)}^T){\mathcal{F}}^T+Q_t\\ & =\mathcal{F}{\bar{P}}_{t-1}{\mathcal{F}}^T+Q_t\end{array}$$
上面式子中由于状态变量和误差变量是无关，所以$E(x_t{\omega }_t)$相关的期望为0，$E({\omega }_t{\omega }_t^T)=Q_t$为噪声协方差。

另外测量方程（也就是测量结果）描述为：
$${\dot{x}}_t=\mathcal{H}\cdot x_t+v_t$$
其中，$\mathcal{H}$代表测量转换矩阵（运动状态空间转换到测量空间），$v_t$代表测量噪声，服从高斯分布。对应的在当前估计结果基础上得到的测量：
$${\dot{x}}_t^{{}^{\prime }}=\mathcal{H}\cdot {\hat{x}}_t$$
这里也通过协方差的形式度量真实测量值和以估计为基准得到测量值的偏差，并套用上面关于状态变量协方差的推导形式，可以得到：
$$\begin{array}{rl}{\dot{P}}_t& =E(({\dot{x}}_t-{\dot{x}}_t^{{}^{\prime }})({\dot{x}}_t-{\dot{x}}_t^{{}^{\prime }}{)}^T)\\ & =\mathcal{H}{\hat{P}}_t{\mathcal{H}}^T+R_t\end{array}$$
在卡尔曼滤波中最后估计值的描述为，其中${\mathcal{K}}_t$为未知且需要估计的量：
$$\begin{array}{rl}{\bar{x}}_t& ={\hat{x}}_t+{\mathcal{K}}_t({\dot{x}}_t-{\hat{x}}_t^{{}^{\prime }})\\ & ={\hat{x}}_t+{\mathcal{K}}_t(\mathcal{H}{x}_t+v_t-\mathcal{H}{\hat{x}}_t)\\ & ={\hat{x}}_t+{\mathcal{K}}_t\mathcal{H}{x}_t+{\mathcal{K}}_t{v}_t-{\mathcal{K}}_t\mathcal{H}{\hat{x}}_t\end{array}$$
那么对应的估计值与真实值的偏差为：
x ˉ t = x ^ t + K t H x t + K t v t − K t H x ^ t x ˉ t = x ^ t + K t H ( x t − x ˉ t ) + K t v t x ˉ t − x t = ( x ^ t − x t ) + K