卡尔曼滤波（Kalman filtering）——通俗易懂版本

引入

  首先请大家思考个问题，假设你手头有两个传感器A和B，测的是同一个信号。可是由于测量误差的原因它们每次的读数都不太一样，怎样才能获取到更准确的数据？

1. 取平均 简单粗暴，直接将两个传感器获得的数据求平均值：$(A+B)/2$，good。
2. 加权平均 假设传感器A精度比传感器B高，那么就可以就可以使用加权平均来计算：$A\ast \omega +B\ast (1-\omega ),\omega \in (0.5,1)$。如果你可以想到加权平均，那么恭喜，你已经掌握卡尔曼滤波的核心了，那就是加权平均。

  看到这里可能有人懵了，卡尔曼到底是啥？下面进行简单的解释：

  以求解小车移动的位置为例，假设你只有一个传感器（GPS），还有一个小车的运动模型。你可以根据运动模型和上一时刻小车的状态计算出下一时刻小车可能在的位置，但也不是那么准。那为了获取小车更精确的位置，下一时刻小车的位置便是把运动模型算出来的值，和传感器测出的值，（就像两个传感器那样），取加权平均，就得到了小车较为精准的位置，这便是卡尔曼滤波要做的事情。

  而卡尔曼滤波就是预测 - 测量之间不断循环迭代。当然，对于某些情况，如GPS + IMU，由于IMU 测量频率远比GPS 高，在只有IMU 测量值时，只执行运动更新，在有GPS 测量值时再进行测量更新。

实例

  在此结合一个具体实例进行讲解。假设有一个智能小车在一条笔直的公路上运行，是一个线性系统。假设智能小车在$t$时刻的运动速度为$v_t$，控制系统的控制输入为$u_t$，所在位置为$p_t$。其上携带有GPS，我们可以从中获取智能汽小车所在的位置，切记GPS是有误差的。

图1 应用场景图

由此可得，在$t$时刻，智能小车的状态我们可以使用一个向量来表示：

$${\mathbf{x}}_t=\left[\begin{array}{c}{p}_t\\ v_t\end{array}\right]$$

  对于方差${\sigma }^2$而言，这里有两个变量$p_t$和$v_t$，所以可以使用协方差矩阵$\mathbf{P}$来表示：
$$\mathbf{P}=\left[\begin{array}{cc}\sum pp& \sum pv\\ \sum vp& \sum vv\end{array}\right]$$

  那么在确定智能小车准确的位置和速度之前，我们先来分析下小车的位置误差。在$t$时刻对于位置而言，其可能会在红色的线段内部。

图2 t时刻小车可能在的位置

  小车的位置在图1中红色线段中的位置可以映射到图2中。图2中波峰所在位置为可能性对大的位置，对应图1中线段的中心位置，也就是高斯分布中心$\mu$。

图3 位置概率分布图

  根据智能小车在直线公路上的运动，我们可以获取其运动方程如下：
$${\mathbf{x}}_t=f({\mathbf{x}}_{t-1},{\mathbf{u}}_t)+{\omega }_t$$
其中,

• $f(x)$是关于${\mathbf{x}}_{t-1},{\mathbf{u}}_t$的函数;
• ${\mathbf{x}}_t$是t时刻的状态向量
• ${\mathbf{u}}_t$是t时刻的控制输入
• ${\omega }_t$是预测误差。

假设这里智能小车系统是一个线性系统，则上述运动可以转化为：
$${\mathbf{x}}_t={\mathbf{F}}_t{\mathbf{x}}_{t-1}+{\mathbf{B}}_t{\mathbf{u}}_t+{\omega }_t$$ (公式1)
其中，

• ${\mathbf{F}}_t$为状态转换方程，将t-1 时刻的状态转换至t 时刻的状态；​
• ${\mathbf{B}}_t$是控制输入矩阵，将运动测量值​ [公式] 的作用映射到状态向量上；​
• ${\omega }_t$是预测的高斯噪声，其均值为0，协方差矩阵为​${\mathbf{Q}}_t$。

根据其GPS的返回值，我们可以获得智能小车的观测方程：
$${\mathbf{z}}_t=\mathbf{H}{\mathbf{x}}_t+{\mathbf{v}}_t$$
其中，

• ${\mathbf{z}}_t$为传感器的测量值；
• ${\mathbf{H}}_t$为转换矩阵，它将状态向量映射到测量值所在的空间中；
• ${\mathbf{v}}_t$为测量的高斯噪声，其均值为0，协方差矩阵为${\mathbf{R}}_t$

状态向量更新

在小车运动过程中，可以通过控制加速度$a$来调整小车的速度：
$$a=\frac{f_t}{m}$$
假设t和t-1时刻之间的时间差为$\Delta t$，则
$$\{\begin{array}{c}{x}_t\text{=}{x}_{t-1}\text{+}{x}_t\Delta \text{t}+\frac{1}{2}\frac{f_t}{m}\Delta t^2\\ v_t=v_{t-1}+\frac{f_t}{m}\Delta t\end{array}$$

那么写成矩阵形式如下：
$$\left[\begin{array}{c}{x}_t\\ v_t\end{array}\right]=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]\left[\begin{array}{c}{x}_{t-1}\\ v_{t-1}\end{array}\right]+\left[\begin{array}{c}\frac{\Delta t^2}{2}\\ \Delta t\end{array}\right]$$
与之前的运动方程对比，可以发现

状态转移矩阵${\mathbf{F}}_t=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]$，控制输入局矩阵${\mathbf{B}}_t=\left[\begin{array}{c}\frac{\Delta t^2}{2}\\ \Delta t\end{array}\right]$

上式就可以写成

${\hat{\mathbf{x}}}_{t|t-1}={\mathbf{F}}_t{\hat{\mathbf{x}}}_{t-1}+{\mathbf{B}}_t{\mathbf{u}}_t$ (公式2)
其中

• ${\hat{\mathbf{x}}}_{t-1}$表示通过t-1时刻系统卡尔曼滤波的状态估计值
• ${\hat{\mathbf{x}}}_{t|t-1}$表示根据t-1时刻的状态估计值，预估出来t时刻的状态预估值

状态向量协方差更新

  系统的误差通过协方差矩阵来表示，根据协方差矩阵的性质：
$Cov(x)=\sum$
$Cov(\mathbf{A}x)=\mathbf{A}\sum {\mathbf{A}}^T$
因此在对状态向量更新后，还需要更新状态向量对应的协方差矩阵$\mathbf{P}$，公式为：
$${\mathbf{P}}_{t|t-1}={\mathbf{F}}_t{\mathbf{P}}_{t-1}{\mathbf{F}}_t^T+{\mathbf{Q}}_t$$
其推导过程如下：
  假设$\mathbf{x}$为t时刻下状态向量的真值（永远未知），由之前的运动方程（公式1）给出，将（公式1）和公式（2）相减可得：
$\begin{array}{rl}{\mathbf{P}}_{t|t-1}& =E[({\mathbf{x}}_t-{\hat{\mathbf{x}}}_{t|t-1})({\mathbf{x}}_t-{\hat{\mathbf{x}}}_{t|t-1}{)}^T]\\ & =E[(\mathbf{F}({\mathbf{x}}_{t-1}-{\hat{\mathbf{x}}}_{t|t-1})+{\omega }_t)\cdot (\mathbf{F}({\mathbf{x}}_{t-1}-{\hat{\mathbf{x}}}_{t|t-1})+{\omega }_t{)}^T]\\ & =\mathbf{F}E[({\mathbf{x}}_{t-1}-{\hat{\mathbf{x}}}_{t|t-1})\cdot ({\mathbf{x}}_{t-1}-{\hat{\mathbf{x}}}_{t|t-1}{)}^T]{\mathbf{F}}^T\\ & +\mathbf{F}E[({\mathbf{x}}_{t-1}-{\hat{\mathbf{x}}}_{t|t-1}){\omega }_t^T]{\mathbf{F}}^T\\ & +\mathbf{F}E[({\omega }_t{\mathbf{x}}_{t-1}-{\hat{\mathbf{x}}}_{t|t-1})]{\mathbf{F}}^T\\ & +E[{\omega }_t{\omega }_t^T]\end{array}$
考虑到状态向量和噪声是不相关的，则$E[({\omega }_t{\mathbf{x}}_{t-1}-{\hat{\mathbf{x}}}_{t|t-1}{)}^T]=0$，因此上式可以化简为：

$\begin{array}{rl}{\mathbf{P}}_{t|t-1}& =\mathbf{F}E[({\mathbf{x}}_{t-1}-{\hat{\mathbf{x}}}_{t|t-1})\cdot ({\mathbf{x}}_{t-1}-{\hat{\mathbf{x}}}_{t|t-1}{)}^T]{\mathbf{F}}^T+E[{\omega }_t{\omega }_t^T]\\ & ={\mathbf{F}}_t{\mathbf{P}}_{t-1}{\mathbf{F}}_t^T+{\mathbf{Q}}_t\end{array}$
推导完毕。

  由此可见，经过预测更新，协方差矩阵$\mathbf{P}$变大了。这主要是因为在原本不确定的位置上，又加上了速度不确定性导致的位置误差，最终使得位置更加不确定。预测更新实际上相当于“加法”：将当前状态转换到下一时刻（并增加一定不确定性），再把外界的干扰（运动测量值）叠加上去（又增加了一点不确定性）。如下图所示。

状态向量更新

  当从智能小车携带的GPS获取到测量值时，那么便可以对状态向量进行更新，也即可以获得t时刻小车最优的状态向量，如下：
$${\hat{\mathbf{x}}}_t={\hat{\mathbf{x}}}_{t|t-1}+{\mathbf{K}}_t({\mathbf{z}}_t-{\mathbf{H}}_t{\hat{\mathbf{x}}}_{t|t-1})$$
$${\mathbf{P}}_t={\mathbf{P}}_{t|t-1}-{\mathbf{K}}_t{\mathbf{H}}_t{\mathbf{P}}_{t|t-1}$$
其中，${\mathbf{K}}_t={\mathbf{P}}_{t|t-1}{\mathbf{H}}_t^T({\mathbf{H}}_t{\mathbf{P}}_{t|t-1}{\mathbf{H}}_t^T+{\mathbf{R}}_t{)}^{-1}$为卡尔曼增益。

推导过程如下：

小车位置1
  从t-1时刻起，小车运动后，经过前面所述的预测更新后，我们就得到了t 时刻的小车位置的估计，由于在卡尔曼滤波中，我们使用高斯概率分布来表示小车的位置，因此这个预测的位置可以写为：
$$y_1(r;{\mu }_1,{\sigma }_1)=\frac{1}{\sqrt{2\pi {\sigma }_1^2}}{e}^{-\frac{{(r-{\mu }_1)}^2}{2{\sigma }_1^2}}$$
  为了与前面的通用的推导区别开来，在这个一维的例子中我们使用了新的符号。不过熟悉高斯概率分布的话应该可以马上看出来，${\mu }_1$ 为这个高斯分布的均值，​ ${\sigma }_1$为方差，而$r$为小车的可能位置， $y_1$ ​ 为某个可能位置 $(r)$ 的概率分布。

小车位置2
  假设在t 时刻，我们通过GPS或测距仪测得小车距离原点的距离$r$，由于测量包含噪声（且在面前我们假设了其为高斯噪声），因此该测量值也可以利用高斯概率分布来表示：

$$y_2(r;{\mu }_2,{\sigma }_2)=\frac{1}{\sqrt{2\pi {\sigma }_2^2}}{e}^{-\frac{{(r-{\mu }_2)}^2}{2{\sigma }_2^2}}$$
除了下标外，其余的字母的含义都和上面的式子一样。

  由此我们得到了小车的两个预估模型，一个是通过上一时刻状态量预测得到的预测模型（预测更新），另一个是通过测量得到的测量模型（测量更新）。两个模型虽然使用不同的方法计算所得，但是两者都反应了小车的位置信息，即两者之间存在一定的关联。如下图所示，蓝色区域是两个预估模型的交界处，也就说明小车位于蓝色区域的概率最大。

&emap;&emap;这也意味着，我们所能得到的关于小车位置的最佳估计，就是将预测更新和测量更新所得的数据融合起来，得到蓝色区域的概率模型。而这里的融合，就是一个简单的“乘法”，并利用了一个性质：两个高斯分布的乘积仍然是高斯分布。

$\begin{array}{rl}{y}_f(r;{\mu }_f,{\sigma }_f)& =y_1(r;{\mu }_1,{\sigma }_1)\cdot y_2(r;{\mu }_2,{\sigma }_2)\\ & =\frac{1}{\sqrt{2\pi {\sigma }_1^2}}{e}^{-\frac{{(r-{\mu }_1)}^2}{2{\sigma }_1^2}}\cdot \frac{1}{\sqrt{2\pi {\sigma }_2^2}}{e}^{-\frac{{(r-{\mu }_2)}^2}{2{\sigma }_2^2}}\end{array}$
上式可以简化为：
$y_f(r;{\mu }_f,{\sigma }_f)=\frac{1}{\sqrt{2\pi {\sigma }_f^2}}{e}^{-\frac{{(r-{\mu }_f)}^2}{2{\sigma }_f^2}}$

其中${\mu }_f$是${\mu }_1$和${\mu }_2$的加权平均，${\sigma }_f$是${\sigma }_1$和${\sigma }_2$的调和平均的二分之一：

${\mu }_f=\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}$

${\sigma }_f=\frac{1}{\frac{1}{{\sigma }_1}\text{+}\frac{1}{{\sigma }_2}}=\frac{{\sigma }_1{\sigma }_2}{{\sigma }_1^2+{\sigma }_2^2}\text{=}{\sigma }_1^2-\frac{{\sigma }_1^4}{{\sigma }_1^2+{\sigma }_2^2}$

  由此我们便可以得到融合后小车位置的高斯分布，如下图中蓝色曲线所示。

转换矩阵$\mathbf{H}$

  在刚刚的一维情况的小例子中，我们其实做了一个隐式的假设，即有预测更新得到的位置的概率分布和测距仪所得的测量值具有相同的单位 (unit)，如米 (m)。

  但实际情况往往不是这样的，比如，测距仪给出的可能不是距离，而是信号的飞行时间（由仪器至小车的光的传播时间），单位为秒 (s)。这样的话，我们就无法直接如上面一般直接将两个高斯分布相乘了。

此时，就需要用到转换矩阵${\mathbf{H}}_t$。由于$r=c\cdot t$，$c$为光速。所以此时$t=\frac{r}{c}$。因此预测值就可以写为：

$y_1(s;{\mu }_1,{\sigma }_1)=\frac{1}{\sqrt{2\pi (\frac{{\sigma }_1}{c}{)}^2}}{e}^{-\frac{{(r-\frac{{\mu }_1}{c})}^2}{2(\frac{{\sigma }_1}{c}{)}^2}}$
而观测值则保持不变：
$y_2(r;{\mu }_2,{\sigma }_2)=\frac{1}{\sqrt{2\pi {\sigma }_2^2}}{e}^{-\frac{{(r-{\mu }_2)}^2}{2{\sigma }_2^2}}$
这样，两个高斯分布在转换矩阵$\mathbf{H}=\frac{1}{c}$的作用下在同一个空间下。根据前面的${\mu }_f$和${\sigma }_f$，可以得到：
$\frac{{\mu }_f}{c}=\frac{{\mu }_1}{c}+\frac{\frac{{\sigma }_1}{c}({\mu }_2-\frac{{\mu }_1}{c})}{(\frac{{\sigma }_1}{c}{)}^2+{\sigma }_2^2}$

${\mu }_f={\mu }_1+\frac{(\frac{{\sigma }_1}{c}{)}^2}{(\frac{{\sigma }_1}{c}{)}^2+{\sigma }_2^2}({\mu }_2-{\mu }_1)$
同理可得
${\sigma }_f={\sigma }_1^2-\mathbf{K}\mathbf{H}{\sigma }_1$

推广

因此，在高维空间中，
$${\hat{\mathbf{x}}}_t={\hat{\mathbf{x}}}_{t|t-1}+{\mathbf{K}}_t({\mathbf{z}}_t-{\mathbf{H}}_t{\hat{\mathbf{x}}}_{t|t-1})$$
$${\mathbf{P}}_t={\mathbf{P}}_{t|t-1}-{\mathbf{K}}_t{\mathbf{H}}_t{\mathbf{P}}_{t|t-1}$$
其中，${\mathbf{K}}_t={\mathbf{P}}_{t|t-1}{\mathbf{H}}_t^T({\mathbf{H}}_t{\mathbf{P}}_{t|t-1}{\mathbf{H}}_t^T+{\mathbf{R}}_t{)}^{-1}$为卡尔曼增益。

参考文献

1. 卡尔曼滤波：从入门到精通
2. 轻松理解卡尔曼滤波