Apollo学习笔记（17）卡尔曼滤波

卡尔曼滤波（KF）和扩展卡尔曼滤波（EKF）是最常用的滤波器优化算法，本文主要是介绍一下卡尔曼滤波，从国外的一篇博客翻译过来的，如果有翻译的不当的地方，烦请指出，最后会给出原文链接，有大佬英文不错的话，推荐直接看原文。

简介卡尔曼滤波

为了更好的说明卡尔曼滤波，假设有一个小机器人在一片树林里，它需要准确的知道自己的位置以便于能够向着目标点移动。

机器人的状态向量（位置和速度）用$x_k$表示，
$$x_k=(p,v)\text{\hspace{1em}(1)}$$

注意状态向量可以是任何东西，只要是能够表示系统当前状态的量，都可以放在状态向量中。在我们的例子中，状态向量是位置和速度，在其他工程中，需要滤波的量可以是油罐中的油量、发动机的温度或者手指在触摸板上的位置，万物皆可模型化，就意味着万物皆可卡尔曼。

机器人是带有一个精度10m的GPS定位，但是在实际情况中，这个精度无法满足机器人的定位需求，森林里面有许多悬崖峭壁，机器人的位置信息哪怕偏差了十厘米，都有可能掉下悬崖，因此仅仅依靠GPS定位信息是无法满足机器人的定位需求的。

我们同时也知道机器人的运动特性，机器人接收到控制指令后，会根据指令控制轮子上的电机调整相应转速，同时，机器人也会根据自身的运动状态推算出自己当前的朝向，理论上来说，是可以根据机器的轮子转速来更新机器人的位置和朝向信息的。然而实际情况是，机器人的运动会受到风的影响，轮子可能会打滑，或者机器人可能经过一段特别崎岖的道路，这些都会对机器人运动状态的估算造成影响。因此，不能单纯的用机器人轮子转动的圈数来表示机器人行驶过的路程，同样的，也不能单纯的用机器人的轮子去推算机器人当前的位置朝向信息。

好吧，目前可以通过两种方式来确定机器人的位置信息，第一种，通过机器人自带的GPS定位，但是这种方式噪声太大，定位精度不够，很容易翻车；第二种方式，通过机器人自身的轮速信息，对机器人现有位置进行预测，这种方式，会因为外界环境的影响，造成对位置的估算不准确。

综合来看，机器人的位置信息，可以有两种方式获得，

1. 通过传感器直接观测，但是传感器测量有误差；
2. 通过机器人运动学方程进行推算，但是会有干扰，会导致机器人位置估算有误差，误差会随着机器人的行驶不断累积，位置精度也会很大。

那么有没有一种方式，能让我们利用所有的可用信息，来提高机器人的定位信息呢？当然有，下面就来介绍一下卡尔曼。

卡尔曼滤波原理

让我们来看一下需要处理的场景。

用一个状态向量表示当前机器人的状态，状态量包含位置和速度两个信息，
$$x=\left[\begin{array}{c}p\\ v\end{array}\right]$$

结合下图来看，坐标系横轴为速度量，纵轴为位置量，我们虽然不知道机器人当前准确的状态信息，但是机器人在下图中某些点的概率更大一些（图中颜色越浅，表示机器人为此状态的概率越大），

卡尔曼滤波主要适用于误差符合高斯分布的随机变量，设定随机变量的均值为$\mu$，方差为${\delta }^2$，如下图所示，

在上图中，位置变量和速度变量两个量之间是不相关的，其中一个变量无论如何变化都不会引起另一个量的变化。
这里只是举个例子，实际情况位置与速度的关系是正相关的，如下图所示，

我们可以根据当前机器人的位置和运动速度去预测机器人下一段时间的位置信息。机器人的运动速度越快，下一段时间内移动的距离就越远；反之，速度越慢，机器人运动的距离就比较近。

两个变量之间的相关性，是一种很重要的性质，因为它能从一个变量的变化来推断出另外一个与之相关变量的变化。同时这也是卡尔曼滤波的主要目的，我们希望能根据所获得的信息最大限度的去确定不确定量的范围。

表示两个变量之间的变化相互影响的关系矩阵，叫做协方差矩阵。矩阵中的元素${\sum }_{ij}$表示行变量上的第$i$个元素与列变量上的第$j$个元素之间的相关度，这里不难看出啊，协方差矩阵是对称的，${\sum }_{ji}$与${\sum }_{ij}$其实是同一个量。

用矩阵来表述问题

有上述分析可知，我们需要估算的状态量都是高斯分布，因此，在进行滤波时，我们需要知道当前时刻$k$的的两个信息量：当前系统状态的最优估计值${\hat{x}}_k$（其平均值为$\mu$），以及此刻的协方差矩阵$P_k$。
$$\begin{gathered} {\hat{x}}_k=\left[\begin{array}{c}position\\ velocity\end{array}\right] \\ {P}_k=\left[\begin{array}{cc}{\sum }_{pp}& {\sum }_{pv}\\ {\sum }_{vp}& {\sum }_{vv}\end{array}\right]\text{\hspace{1em}(1)} \end{gathered}$$

这里我们只用了位置和速度两个量，当然也可有根据实际情况的需求增加更多的状态量来表示系统的状态，这里只是举个例子做个说明。

下面，我们需要找到入一种从当前时刻$k-1$系统状态去预测下一个时刻$k$系统状态的方法。要注意的是，卡尔曼滤波对系统的初始状态并不是要求十分精确。

我们用$F_k$表示系统状态转移矩阵，如下图所示，

通过状态转移矩阵，可以将蓝色区域内的点移动到红色的区域。用卡尔曼的方式来表达就是，蓝色区域为当前状态，红色区域为预测状态。

下面来说一下如何应用上述理论，如何通过状态转移矩阵去预测下一时刻机器人的位置和速度？

首先，建立机器人运动方程：
$$\begin{gathered} p_k=p_{k-1}+v_{k-1}\Delta t \\ {v}_k=v_{k-1} \end{gathered}$$

写成矩阵的形式，

$${\hat{x}}_k=\left[\begin{array}{c}{p}_k\\ v_k\end{array}\right]=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]\left[\begin{array}{c}{p}_{k-1}\\ v_{k-1}\end{array}\right]=F_k{\hat{x}}_{k-1}\text{\hspace{1em}(2)}$$

现在，我们可以通过公式（2）对机器人的下一个状态进行预测，但是此时仍然不知道如何去更新协方差矩阵。如果状态向量中的每个状态乘以一个矩阵$A$，那么协方差矩阵会如何变化？

看一下下面的公式，
$$\begin{gathered} Cov(x)=\sum \\ Cov(Ax)=A\sum A^T\text{\hspace{1em}(3)} \end{gathered}$$

综上，有

$$\begin{gathered} {\hat{x}}_k=F_k{\hat{x}}_{k-1} \\ {P}_k=F_k{P}_{k-1}{F}_k^T\text{\hspace{1em}(4)} \end{gathered}$$
注意，这里的$P$是协方差矩阵，不是位置。

外部影响

上述的推导过程是没有考虑外部的影响的，机器人在行走的过程中，难免会受到外界环境的影响，这些影响都会影响到机器人的位置估算。

举个例子，给火车建立运动学模型时，火车司机加了油门后，火车就会加速行驶。同样的，在我们的机器人的例子中，机器人导航也一样会下发命令给机器人的驱动电机，从而使得机器人按照期望去运动。如果我们知道可能会对机器人运动产生影响的因素，将这些因素都考虑进来，可以提高机器人位置预测的准确性，将外界可能对机器人运动有影响的约束向量用$u_k$表示。

在此文使用的例子中，能对机器人的运动造成影响的向量就是加速度$a$，由此，可以得到机器人的动力学模型，
$$\begin{gathered} p_k=p_{k-1}+v_{k-1}\Delta t+\frac{1}{2}a\Delta t^2 \\ {v}_k=v_{k-1}+a\Delta t \end{gathered}$$

转化成矩阵形式，

$${\hat{x}}_k=F_k{\hat{x}}_{k-1}+\left[\begin{array}{c}\frac{\Delta t^2}{2}\\ \Delta t\end{array}\right]a=F_k{\hat{x}}_{k-1}+B_k{u}_k\text{\hspace{1em}(5)}$$

式中，$B_k$为控制矩阵，$u_k$为控制向量。（这里只是举个例子，所以用的都是很简单的模型，这里只有加速度一个外部影响，当然实际情况，会根据需要添加别的量。）

外部的不确定量

如果机器人完全按照动力学方程去运动，那就没卡尔曼滤波什么事了。另外，如果我们知道所有会对机器人运动产生影响的外界因素，并将其考虑进来，计算出所产生力的影响，那也能很准确的推算出机器人的位置。

但是，总有些无法考虑到的外部影响因素，这些因素怎么处理呢？就好像无人机的定位一样，无人机很容易受到风力的影响，想想怎么对风力建模，脑袋都大。同样的，如果是机器人的话，机器人的轮子难免发生打滑，碰到地面等，都会对机器人的移动造成影响。我们没法将所有的外界的影响都考虑进来，但是，如果任意一个外界不可控的因素发生时，我们对机器人的位置预测都会不准确。

为了解决这个问题，我们会在每一次预测中增加一个扰动量，来表示这些无法避免的且不好建模的外界影响因素。

机器人的每个状态量都可以从初始时的状态（$x_{k-1}$）转移到新的状态范围内（$x_k$）。

$k-1$时刻机器人状态可以转移到一个新的范围内，这个状态范围符合高斯分布且其方差为$Q_k$，换句话说，就是我们把可能造成系统扰动的外部环境因素当做一个方差为$Q_k$的白噪声来处理，如下图所示，

由于系统加入了白噪声，这样就产生了一个新的高斯分布（与原来的系统有着相同的期望，但是方差不同）。

在加入白噪声$Q_k$之后，有预测方程，
$$\begin{gathered} {\hat{x}}_k=F_k{\hat{x}}_{k-1}+B_k{u}_k \\ {P}_k=F_k{P}_{k-1}{F}_k^T+Q_k\text{\hspace{1em}(6)} \end{gathered}$$

新的系统状态${\hat{x}}_k$是从上一时刻的系统状态${\hat{x}}_{k-1}$和输入的系统控制量$u_k$计算出来的系统估计值，此估值也叫做先验估计值，是各个变量高斯分布的均值。另外，$P_k$为协方差矩阵，表示系统的不确定性，是由上一时刻的协方差矩阵和当前时刻外部噪声的协方差一起计算得到的。

好，到目前位置，我们已经有了系统状态方程，可以通过此方程去预测系统下一时刻的状态${\hat{x}}_k$和协方差$P_k$。另外，我们还有机器人的传感器这个信息没有使用呢，如果加上GPS信号，是不是可以进一步的提高机器人的定位精度？

传感器的观测值

一个机器人可能有很多个传感器，这些传感器可以帮助我们更好的了解当前系统的状态。有的传感器可能返回位置信息，有的返回速度信息，每个传感器会都会监测系统的某一个状态，并周期性的返回所监测的系统状态。

需要注意的是，传感器返回值的单位可能和我们预测使用的单位是不一致的，要注意保持一致。下面，我们需要找到真实值和测量值之间的关系矩阵$K_k$。

我们能够计算出传感器返回值的分布情况，如下所示，
$$\begin{gathered} {\mu }_{expected}=H_k{\hat{x}}_k \\ \sum _{expected}=H_k{P}_k{H}_k^T\text{\hspace{1em}(7)} \end{gathered}$$

卡尔曼滤波之所以好用，主要是因为它可以很好的处理传感器的噪声。换句话说，就是我们的传感器返回值是不能完全相信的，我们每次预测的系统状态也是在某个有效范围内，并不是完完全全的真值。

通过传感器每次返回的观测值，我们可以大致估算出当前时刻的系统状态。但是，观测值是存在噪声的，所以估算出的一些状态是比另一些估算出的状态可能性更高。

我们使用$R_k$来表示传感器噪声的协方差。传感器噪声同样也符合高斯分布，其均值就是传感器读到值的平均值，用$z_k$表示。

好，到目前为止，我们已经有了两个高斯分布：一个是预测带来的，另一个就是传感器返回值带来的噪声。

如图所示，其中红色的部分为根据模型进行的预测，绿色部分为传感器返回的值。我们现在需要对预测的值和传感器的返回值进行相关的处理，得到一个更为准确的系统状态信息。

那么如何使用这两个信息来提高机器人当前的位置准确性呢？

对于任何从传感器上读到任意一组值$(z_1,z_2)$，都有下面两种可能：

1. 传感器的观测值$z_k$有问题，与通过模型预测的分布相差很大，没法很好的表示机器人的状态；
2. 传感器的观测值$z_k$没有问题，与通过模型预测的分布相差相同，可以很好的表示机器人的状态。

假如现在我们面临这两种可能，我们会认为这两种情况都是正确的，将这两个分布相乘就可以得到一个新的分布，如下所示：

注意看图中高亮的部分，是两个分布重叠的部分，这个新的分布比之前的两个分布都要更加精确（单独用模型去预测和用传感器去测量）。因此，新的分布是结合了两种方式对机器人当前状体的最好估计。

如下图所示，新的分布是不是像一个与之前两个高斯分布不同的新的高斯分布，

到此为止，可以知道，把两个不同的高斯分布（各自有自己的均值和协方差矩阵）相乘，可以得到一个新的高斯矩阵，新的高斯矩阵有新的均值和协方差矩阵！也许你会发现，我们可以通过原来的两个高斯分布参数计算出新的高斯分布的参数。

结合高斯分布

现在我们就来推导出如何从原来的高斯分布参数计算出新的高斯分布的参数。

首先，从一维来分析是最简单的，如下图所示，是一个均值为$\mu$，方差为${\delta }^2$的高斯分布函数，
$$N(x,\mu ,\delta )=\frac{1}{\sigma \sqrt{2\pi }}{e}^{-\frac{(x-\mu {)}^2}{2{\sigma }^2}}\text{\hspace{1em}(8)}$$

其实我们最想知道的就是，把两个高斯分布相乘之后得到一个新的高斯分布，如何从之前的分布参数计算出新高斯分布的参数。如下图所以，蓝色的曲线是由红色和绿色曲线所代表的高斯分布相乘得来，

$$N(x,{\mu }_0,{\delta }_0)\cdot N(x,{\mu }_1,{\delta }_1)\stackrel{?}{=}N(x,{\mu }^{{}^{\prime }},{\delta }^{{}^{\prime }})\text{\hspace{1em}(9)}$$

将公式（８）带入公式（９）并化简之后有，
$$\begin{gathered} {\mu }^{{}^{\prime }}={\mu }_0+\frac{{\sigma }_0^2({\mu }_1-{\mu }_0)}{{\sigma }_0^2+{\sigma }_1^2} \\ {\sigma }^{{}^{\prime }2}={\sigma }_0^2-\frac{{\sigma }_0^4}{{\sigma }_0^2+{\sigma }_1^2}\text{\hspace{1em}(10)} \end{gathered}$$

令

$$k=\frac{{\sigma }_0^2}{{\sigma }_0^2+{\sigma }_1^2}\text{\hspace{1em}(11)}$$

可以把上式简化为，
$$\begin{gathered} {\mu }^{{}^{\prime }}={\mu }_0+k({\mu }_1-{\mu }_0) \\ {\sigma }^{{}^{\prime }2}={\sigma }_0^2-k{\sigma }_0^2\text{\hspace{1em}(12)} \end{gathered}$$

令$\sum$为高斯分布的协方差，$\mu$为高斯分布的均值，则式（11）（12）可以化简为，
$$K={\sum }_0({\sum }_0+{\sum }_1{)}^{-1}\text{\hspace{1em}(13)}$$

$$\begin{gathered} {\mu }^{{}^{\prime }}={\mu }_0+K({\mu }_1-{\mu }_0) \\ {\sum }^{{}^{\prime }}={\sum }_0-K{\sum }_0\text{\hspace{1em}(14)} \end{gathered}$$

式中$K$就是卡尔曼增益，下面我们就会用到这个量。

整合

我们有两个正态分布：

1. 一个是预测正态分布，其参数为$({\mu }_0,{\sum }_0)=(H_k{\hat{x}}_k,H_k{\hat{x}}_k{H}_k^T)$；
2. 另一个是观测正态分布，其参数为$({\mu }_1,{\sum }_1)=(z_k,R_k)$。

直接带入公式（14）有，
$$\begin{gathered} H_k{\hat{x}}_k^{{}^{\prime }}=H_k{\hat{x}}_k+K(z_k-H_k{\hat{x}}_k) \\ {H}_k{P}_k^{{}^{\prime }}{H}_k^T=H_k{P}_k{H}_k^T-K{H}_k{P}_k{H}_k^T\text{\hspace{1em}(15)} \end{gathered}$$

从式（13）可以推出卡尔曼增益为，
$$K=H_k{P}_k{H}_k^T(H_k{P}_k{H}_k^T+R_k{)}^{-1}\text{\hspace{1em}(16)}$$

把式（15）（16）中的$H_k$化简掉，有
$$\begin{gathered} {\hat{x}}_k^{{}^{\prime }}={\hat{x}}_k+K^{{}^{\prime }}(z_k-H_k{\hat{x}}_k) \\ {P}_k^{{}^{\prime }}=P_k-K^{{}^{\prime }}{H}_k{P}_k\text{\hspace{1em}(17)} \end{gathered}$$

$$K^{{}^{\prime }}=P_k{H}_k^T(H_k{P}_k{H}_k^T+R_k{)}^{-1}\text{\hspace{1em}(18)}$$

这就是卡尔曼滤波剩下的三个公式了。

式中${\hat{x}}_k^{{}^{\prime }}$是得到的新的最优状态估计，$P_k^{{}^{\prime }}$则为当前新的最优状态估计的协方差矩阵。

将${\hat{x}}_k^{{}^{\prime }}$和$P_k^{{}^{\prime }}$当做下一个卡尔曼周期的输入。

卡尔曼滤波的工作流程大致如下所示，

总结

卡尔曼滤波总共有五个公式，为

1. 预测步骤： $$\begin{gathered} {\hat{x}}_k=F_k{\hat{x}}_{k-1}+B_k{u}_k \\ {P}_k=F_k{P}_{k-1}{F}_k^T+Q_k\text{\hspace{1em}(6)} \end{gathered}$$

2. 更新步骤：

   $$\begin{gathered} {\hat{x}}_k^{{}^{\prime }}={\hat{x}}_k+K^{{}^{\prime }}(z_k-H_k{\hat{x}}_k) \\ {P}_k^{{}^{\prime }}=P_k-K^{{}^{\prime }}{H}_k{P}_k\text{\hspace{1em}(17)} \end{gathered}$$

   $$K^{{}^{\prime }}=P_k{H}_k^T(H_k{P}_k{H}_k^T+R_k{)}^{-1}\text{\hspace{1em}(18)}$$

式中，

${\hat{x}}_k$为$k$时刻的系统状态，

$F_k$为$k$时刻的系统状态转移矩阵（为什么是$k$时刻的，看了EKF 就明白了），

$u_k$为$k$时刻，当前系统的控制输入量，

$B_k$为$k$时刻系统的控制输入矩阵，

$P_k$为$k$时刻系统的协方差矩阵，

$Q_k$为$k$时刻预测噪声协方差矩阵，

$R_k$为$k$时刻测量噪声协方差矩阵，

$H_k$为$k$时刻观测矩阵，

${\hat{z}}_k$为$k$时刻的观测值，

$K^{{}^{\prime }}$为当前时刻的卡尔曼增益。

卡尔曼滤波适用于任何线性系统，对于非线性系统，可以使用扩展卡尔曼滤波(EKF)，后面会有详细的文章。

原文链接：
http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/#mathybits