卡尔曼滤波推导

参考论文：
http://www.cl.cam.ac.uk/~rmf25/papers/Understanding%20the%20Basis%20of%20the%20Kalman%20Filter.pdf
知乎上Kent Zeng博士的回答言简意赅，当时看了后觉得醍醐灌顶，但是还是想自己推导一遍，所以就参照了上面这篇论文。
这篇博客也是为了练习markdown的语法，所以大神可以略过，有错误请指正，求轻喷:-D
1.卡尔曼滤波典型应用：
（1）平滑噪声数据；
（2）参数估计。
（3）实例：阿波罗计划，卫星导航设备，智能手机，电脑游戏。
2.高斯分布的重要特性： 两个高斯分布的乘积仍然是一个高斯分布。
3.数学模型
（1）运动方程：
$${\mathbf{x}}_t={\mathbf{F}}_t{\mathbf{x}}_{t-1}+{\mathbf{B}}_t{\mathbf{u}}_t+{\mathbf{w}}_t\text{(1)}$$
${\mathbf{x}}_t$——状态向量，包含系统有意义的项，例如t时刻的位置，速度，朝向等。
${\mathbf{u}}_t$——控制输入向量，例如航向角，节流阀设置，抱闸力等。
${\mathbf{F}}_t$——状态转换矩阵，t-1时刻的状态对t时刻的系统状态的影响，例如t-1时刻的位置和速度都会影响t时刻的系统状态。
${\mathbf{B}}_t$——控制输入矩阵，控制输入参数对状态向量的影响。例如，节流阀的设置对系统速度和位置的影响。
${\mathbf{w}}_t$——状态向量中每个参数的过程噪声。该噪声是一个零均值多元高斯正太分布，协方差矩阵为${\mathbf{Q}}_t$。
（2）测量方程：
$${\mathbf{z}}_t={\mathbf{H}}_t{\mathbf{x}}_t+{\mathbf{v}}_t\text{(2)}$$
${\mathbf{z}}_t$——测量向量。
${\mathbf{H}}_t$——转换矩阵，将状态向量参数映射到测量域上。
${\mathbf{v}}_t$——测量向量噪声。和过程噪声一样，该噪声也是零均值的高斯白噪声，协方差矩阵为${\mathbf{R}}_t$。
4.实例推导
实例：一维的跟踪问题，火车沿着铁轨运动。
状态向量${\mathbf{x}}_t$包含火车的位置和速度：$$x_t=\left(\begin{array}{c}{x}_t\\ \dot{x_t}\end{array}\right)$$
列车驱动可能会施加刹车或者加速输入给系统，可以用牛顿方程，用一个外力$f_t$和列车质量m表示，控制向量${\mathbf{u}}_t$为：${\mathbf{u}}_t=\frac{f_t}{m}$

∆t时间段内，驱动力和列车的位置和速度的关系可以列为如下方程：
$x_t=x_{t-1}+(\dot{x_{t-1}}\times \Delta t)+\frac{f_t(\Delta t{)}^2}{m}$
$\dot{x_t}=\dot{x_{t-1}}+\frac{f_t\Delta t}{2m}$

这些线性方程可以写为如下矩阵形式：
$\left(\begin{array}{c}{x}_t\\ \dot{x_t}\end{array}\right)=\left(\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right)\left(\begin{array}{c}{x}_{t-1}\\ \dot{x_t-1}\end{array}\right)+\left(\begin{array}{c}\frac{(\Delta t{)}^2}{2}\\ \Delta t\end{array}\right)\frac{f_t}{m}$

对照方程（1）$x_t=F_t{x}_{t-1}+B_t{u}_t+w_t$，可以得到：
${\mathbf{F}}_t=\left(\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right)and{\mathbf{B}}_t=\left(\begin{array}{c}\frac{(\Delta t{)}^2}{2}\\ \Delta t\end{array}\right)$

系统的真实状态$x_t$不能直接测量，卡尔曼滤波提供可以确定估计量$x_t$的算法，通过结合系统模型和确定值或者线性方程的噪声参数。状态向量的参数估计值可以通过概率密度函数提供，而不是离散值。卡尔曼滤波是基于高斯概率密度函数的，为了完整描述高斯方程，我们需要知道他们的变量协方差，这些都写在矩阵$P_t$中，$P_t$的主对角线上的元素对应状态向量的变量，非对角线的元素对应于状态向量的变量与变量之间的协方差。
卡尔曼滤波算法包含两个阶段：预测和更新
标准卡尔曼滤波算法的**预测过程**为：
$$\widehat{{\mathbf{x}}_{t|t-1}}={\mathbf{F}}_t\widehat{{\mathbf{x}}_{t-1|t-1}}+{\mathbf{B}}_t{\mathbf{u}}_t\text{(3)}$$
$${\mathbf{P}}_{t|t-1}={\mathbf{F}}_t{\mathbf{P}}_{t-1|t-1}{\mathbf{F}}_t^{\mathbf{T}}+{\mathbf{Q}}_t\text{(4)}$$

其中，Q_t是过程噪声的协方差，也就是控制噪声。前一部分是对（3）式的推导，下面推导（4）式。
未知的真值$x_t$的预测值${\hat{x}}_{t|t-1}$的方差可以写为：$P_{t|t-1}=E[(x_t-{\hat{x}}_{t|t-1})(x_t-{\hat{x}}_{t|t-1}{)}^T]$
看下(1)式$x_t=F_t{x}_{t-1}+B_t{u}_t+w_t$，以及(3)式，可以得到：$$x_t-{\hat{x}}_{t|t-1}=F_t(x_{t-1}-{\hat{x}}_{t-1|t-1})+w_t$$
$⟹P_{t|t-1}=E[(F_t(x_{t-1}-{\hat{x}}_{t-1|t-1})+w_t)(F_t(x_{t-1}-{\hat{x}}_{t-1|t-1})+w_t{)}^T]=E[F_t(x_{t-1}-{\hat{x}}_{t-1|t-1})(x_{t-1}-{\hat{x}}_{t-1|t-1}{)}^T{F}_t^T+F_t(x_{t-1}-{\hat{x}}_{t-1|t-1})w_t^T+w_t(x_{t-1}-{\hat{x}}_{t-1|t-1}{)}^T{F}_t^T+w_t{w}_t^T]=F_{t}E[(x_{t-1}-{\hat{x}}_{t-1|t-1})(x_{t-1}-{\hat{x}}_{t-1|t-1}{)}^T]F_t^T+F_{t}E[(x_{t-1}-{\hat{x}}_{t-1|t-1})w_t^T]+E[w_t(x_{t-1}-{\hat{x}}_{t-1|t-1}{)}^T]F_t^T+E[w_t{w}_t^T]$
需要注意，状态估计误差与控制过程噪声是不相关的，因此：
$$E[(x_{t-1}-{\hat{x}}_{t-1|t-1})w_t^T]+E[w_t(x_{t-1}-{\hat{x}}_{t-1|t-1}{)}^T]=0$$
因此：
$P_{t|t-1}=E[F_t(x_{t-1}-{\hat{x}}_{t-1|t-1})(x_{t-1}-{\hat{x}}_{t-1|t-1}{)}^T{F}_t^T+w_t{w}_t^T]$
$⟹P_{t|t-1}=F{P}_{t-1|t-1}{F}^T+Q_t$
标准卡尔曼滤波算法的**测量更新过程**为：
$${\hat{\mathbf{x}}}_{t|t}={\hat{\mathbf{x}}}_{t|t-1}+{\mathbf{K}}_t({\mathbf{z}}_t-{\mathbf{H}}_t){\hat{\mathbf{x}}}_{t|t-1}\text{(5)}$$
$${\mathbf{P}}_{t|t}={\mathbf{P}}_{t|t-1}-{\mathbf{K}}_t{\mathbf{H}}_t{\mathbf{P}}_{t|t-1}\text{(6)}$$
其中：
$${\mathbf{K}}_t={\mathbf{P}}_{t|t-1}{\mathbf{H}}_t^{\mathbf{T}}({\mathbf{H}}_t{\mathbf{P}}_{t|t-1}{\mathbf{H}}_t^{\mathbf{T}}+{\mathbf{R}}_t{)}^{-1}\text{(7)}$$
Solution:
卡尔曼滤波器推导中，考虑一个简单的一维跟踪问题，比如列车沿着一条铁轨线运动的例子。每一个测量时间点上，我们期望知道列车位置（或者更精确点，是列车顶部天线的安装位置）的最优估计。
已知的信息是：（1）列车上一次的位置和速度；（2）无线电测量系统的测量值。通过将预测值和测量值结合，得到列车定位的最可能估计，如图1所示。

系统的初始状态（t=0s）状态已知，精确度合理，如图2所示，列车的位置是一个高斯分布。

在下一个时刻（t=1s），我们可以基于已知的限制条件估计列车的新位置，例如t=0s时刻的位置和速度，最大可能加速度等。实际中，我们可能还知道控制输入，例如刹车制动器和加速装置的输入。无论哪种清理，我们都可以预估列车的一个新位置，如图3所示，列车的位置用一个新的高斯分布来表示。注意，我们可以看到，图3的高斯分布比图1的高斯分布矮胖了很多，这是因为有噪声在里面，不确定性变大了。数学推导上，可以参照式（1）的推导。方差变大，可以参照式（4）的推导，这表示相对t=0时刻，我们减少了精度，因为任何加速度或减速都会带来噪声影响。

在t=1时刻，我们也用无线电定位系统测量了列车的位置，如图4所示，蓝色的高斯分布代表了这个测量者，通过结合预测值和测量值，我们能得到的一个最优估计值。可以通过把两个高斯分布相乘，如图5所示。

**高斯分布的一个关键特性是：两个高斯分布的乘积仍然是一个高斯分布。**它允许无限多个高斯分布相乘，但是结果并不会将高斯分布的项变复杂。因此，经过一段时间后，新的概率分布仍然是一个高斯分布，这就是卡尔曼滤波器迭代特性的关键。
下面推导卡尔曼滤波的测量更新过程。
图3中代表预测分布的红色高斯分布可以表达为：
$$y_1(r;{\mu }_1,{\sigma }_1)\triangleq \frac{1}{\sqrt{2\pi {\sigma }_1^2}}{e}^{-\frac{(r-{\mu }_1{)}^2}{2{\sigma }_1^2}}\text{(8)}$$
图4中代表测量分布的蓝色高斯分布可以表达为：
$$y_2(r;{\mu }_2,{\sigma }_2)\triangleq \frac{1}{\sqrt{2\pi {\sigma }_2^2}}{e}^{-\frac{(r-{\mu }_2{)}^2}{2{\sigma }_2^2}}\text{(9)}$$
如图5所示，将两个高斯分布相乘，新的高斯分布表达了预测和更新的融合结果，而且系统当前估计的最优值，可以由两个高斯函数的乘积给出：
$$y_1(r;{\mu }_1,{\sigma }_1，{\mu }_2,{\sigma }_2)\triangleq \frac{1}{\sqrt{2\pi {\sigma }_1^2}}{e}^{-\frac{(r-{\mu }_1{)}^2}{2{\sigma }_1^2}}\frac{1}{\sqrt{2\pi {\sigma }_2^2}}{e}^{-\frac{(r-{\mu }_2{)}^2}{2{\sigma }_2^2}}=\frac{1}{\sqrt{2\pi {\sigma }_1^2{\sigma }_2^2}}{e}^{-(\frac{(r-{\mu }_1{)}^2}{2{\sigma }_1^2}+\frac{(r-{\mu }_2{)}^2}{2{\sigma }_2^2})}\text{(10)}$$
将上式的二次项展开，然后整式可以重写为：
$$y_{fused}(r;{\mu }_{fused},{\sigma }_{fused})\triangleq \frac{1}{\sqrt{2\pi {\sigma }_{fused}^2}}{e}^{-\frac{(r-{\mu }_{fused}{)}^2}{2{\sigma }_{fused}^2}}\text{(11)}$$
其中，
$${\mu }_{fused}=\frac{{\mu }_1{\sigma }_2^2+{\mu }_2{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2}={\mu }_1+\frac{{\sigma }_1^2({\mu }_2-{\mu }_1)}{{\sigma }_1^2+{\sigma }_2^2}\text{(12)}$$
$${\sigma }_{fused}^2=\frac{{\sigma }_1^2{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}={\sigma }_1^2-\frac{{\sigma }_1^4}{{\sigma }_1^2+{\sigma }_2^2}\text{(13)}$$
上面两个方程表示了卡尔曼滤波器的测量更新步骤，之后会详细展示。然而，为了展示一个更一般的情况，我们需要对这个例子扩展一下。
在上述例子中，是假设预测和更新在相同的坐标系和相同的单位下进行的，这样就形成了更简洁的预测和更新步骤。然后实际中，很重要的是将预测和更新都映射到同一个域中。在这个例子中，列车的位置直接预测为一个沿着铁轨线的距离，单位是米，但是时间测量单位是秒s。为了允许将测量和更新的概率分布相乘，一个值必须转换到另一个域中，而常规做法是，将预测值映射到测量域中，通过转换矩阵$H_t$。
现在重新看下公式（8）和公式（9），这两个式子都是让$y_1$和$y_2$表示沿着轨道的值，单位是米。现在，将$y_2$表示为，处在x=0位置处的无线电发射装置，将信号发射到列车顶部的无线电所用的飞行时间。空间域分布$y_1$通过光速因子c映射到测量域上，因此公式（8）和（9）可以重写为：
$$y_1(r;{\mu }_1,{\sigma }_1,c)\triangleq \frac{1}{\sqrt{2\pi (\frac{{\sigma }_1}{c}{)}^2}}{e}^{-\frac{(r-\frac{{\mu }_1}{c}{)}^2}{2(\frac{{\sigma }_1}{c}{)}^2}}\text{(14)}$$
图4中代表测量分布的蓝色高斯分布可以表达为：
$$y_2(r;{\mu }_2,{\sigma }_2)\triangleq \frac{1}{\sqrt{2\pi {\sigma }_2^2}}{e}^{-\frac{(r-{\mu }_2{)}^2}{2{\sigma }_2^2}}\text{(15)}$$
现在，两个分布都被定义在测量域中，无线电信号沿着时间轴s轴传播，测量单位为秒。按照之前的推导结果，可以发现：
$$\frac{{\mu }_{fused}}{c}=\frac{{\mu }_1}{c}+\frac{(\frac{{\sigma }_1}{c}{)}^2({\mu }_2-\frac{{\mu }_1}{c})}{(\frac{{\sigma }_1}{c}{)}^2+{\sigma }_2^2}$$
$${\mu }_{fused}={\mu }_1+(\frac{\frac{{\sigma }_1^2}{c}}{})+\frac{{\sigma }_1^2({\mu }_2-{\mu }_1)}{{\sigma }_1^2+{\sigma }_2^2}\text{(16)}$$

令$H=\frac{1}{c}$，$K=\frac{H{\sigma }^2}{H^2{\sigma }_1^2+{\sigma }_2^2}$，从而得到：
$${\mu }_{fused}={\mu }_1+K\cdot ({\mu }_2-H{\mu }_1)\text{(17)}$$
类似的，可以得到融合后的方差：
$$\frac{{\sigma }_{fused}^2}{c^2}=(\frac{{\sigma }_1}{c}{)}^2-\frac{(\frac{{\sigma }_1}{c}{)}^4}{(\frac{{\sigma }_1}{c}{)}^2+{\sigma }_2^2}$$
$$\Rightarrow {\sigma }_{fused}^2={\sigma }_1^2-\left(\frac{\frac{{\sigma }_1^2}{c}}{{\left(\frac{{\sigma }_1}{c}\right)}^2+{\sigma }_2^2}\right)\frac{{\sigma }_1^2}{c}={\sigma }_1^2-KH{\sigma }_1^2\text{(18)}$$
现在，和Kalman滤波器的更新方程$(5)(6)(7)$对比一下，可以得到：

• ${\mu }_{fused}\to {\hat{\mathbf{x}}}_{t|t}$融合后的状态向量；
• ${\mu }_1\to {\hat{\mathbf{x}}}_{t|t-1}$融合前的状态向量，也就是预测的状态向量；
• ${\sigma }_1^2\to {\mathbf{P}}_{t|t}$数据融合后的协方差矩阵（置信度）；
• ${\sigma }_1^2\to {\mathbf{P}}_{t|t-1}$数据融合前的协方差矩阵（置信度）；
• ${\mu }_2\to {\mathbf{z}}_t$观测向量；
• ${\sigma }_2^2\to {\mathbf{R}}_t$测量噪声的矩阵（置信度）；
• $H\to {\mathbf{H}}_t$转换矩阵，用于将状态向量参数映射到测量域；
• $K=\frac{H{\sigma }_1^2}{H^2{\sigma }_1^2+{\sigma }_2^2}\to {\mathbf{K}}_t={\mathbf{P}}_{t|t-1}{\mathbf{H}}_t^{\mathbf{T}}({\mathbf{H}}_t{\mathbf{P}}_{t|t-1}{\mathbf{H}}_t^{\mathbf{T}}+{\mathbf{R}}_t{)}^{-1}$Kalman增益。
  现在，可以很容易观察标准Kalman滤波方程和(17)（18）之间的关系：
  $${\mu }_{fused}={\mu }_1+\left(\frac{H{\sigma }_1^2}{H^2{\sigma }_1^2+{\sigma }_2^2}\right)\cdot ({\mu }_2-H{\mu }_1)$$
  $$\to {\hat{\mathbf{x}}}_{t|t}={\hat{\mathbf{x}}}_{t|t-1}+{\mathbf{K}}_t({\mathbf{z}}_t={\mathbf{H}}_t{\hat{\mathbf{x}}}_{t|t-1})$$
  $${\sigma }_{fused}^2={\sigma }_1^2-\left(\frac{H{\sigma }_1^2}{H^2{\sigma }_1^2+{\sigma }_2^2}\right)H{\sigma }_1^2$$
  $$\to {\mathbf{P}}_{t|t}={\mathbf{P}}_{t|t-1}-{\mathbf{K}}_t{\mathbf{H}}_t{\mathbf{P}}_{t|t-1}$$