大家好，好久不见，我是小政。读研期间，我的研究方向是协作定位，涉及到多机器人分布式融合，主要用到了卡尔曼滤波，CI融合等概念。卡尔曼滤波我也是研究了很久，一直在思考的问题就是，卡尔曼滤波到底有什么用？如果用笼统的话来解释，那就是：将预测的值和量测的值结合起来，获得更接近真实情况的数值。

一、问题提出

  假设有一个机器人正在往前方行驶，为让机器人实现导航，机器人需要知道自身当前所在的位置，即机器人存在一个位置信息和速度信息的状态 ${\vec{x}}_k=\left(\vec{p},\vec{v}\right)$

图1 机器人移动轨迹

  机器人自身带有GPS传感器，定位精度为10米，但传感器都是存在误差，况且路上可能存在小石子，也会存在阻力，如果机器人稍微偏出几米，那么就可能出现滑到等情况，所以GPS提供的信息存在一定误差。

  同时，我们也可以预测机器人的移动轨迹：给它发送指令，让电机按固定速率往一个方向移动，但机器人对于自身的状态是未知的，它可能会出现轮子打滑等情况，所以车轮转动的次数并不能代表机器人实际移动的距离，所以这个距离的预测也是存在误差的。
  综合以上两点，GPS传感器测量的信息存在误差，预测的机器人移动距离也存在误差，所以我们能否在这两者基础之上，得到一个效果更好的预测，使得它的准确度比机器人搜集单次预测汇总更好？用卡尔曼滤波，就可以将这个问题解决。

二、解决思路

  机器人自身有一个状态，它和位置、速度有关：
$$\vec{x}=\left[\begin{array}{c}p\\ v\end{array}\right]$$

  卡尔曼滤波假设两个变量（位置$p$和速度$v$）是随机的，且符合高斯分布，每个变量有一个均值$\mu$，它是随机分布的中心；有一个方差${\sigma }^2$，它衡量组合的不确定性。

  如果位置和速度不是相关的，意味着不能从一个变量推导到另一个变量。如果位置和速度是相关的，机器人前往特定的位置就取决于它自身的速度。

  这不难理解，如果基于旧位置估计新位置，产生两个结论：如果速度很快，机器人可能移动得更远，所以得到的位置会更远；如果速度很慢，机器人就走不了那么远。

  这种关系对目标跟踪来说非常重要，因为它提供了更多信息：一个可以衡量可能性的标准。这就是卡尔曼滤波的目标：从不确定信息中挤出尽可能多的信息！

  为了捕获这种相关性，我们用的是协方差矩阵。简而言之，矩阵的每个值是第$i$个变量和第$j$个变量之间的相关程度（由于矩阵是对称的，$i$和$j$的位置可以随便交换）。我们用$\Sigma$表示协方差矩阵，在这个例子中，就是${\Sigma }_{ij}$。

图2 协方差矩阵相关性

三、问题建模

  把以上关于状态的信息建模为高斯分布（灰色部分），同时我们还需要获得$k$时刻的两个信息：最佳估计${\hat{x}}_k$，协方差矩阵$P_k$。
$$\begin{array}{rl}{\hat{x}}_k& =\left[\begin{array}{c}p\\ v\end{array}\right]\\ P_k& =\left[\begin{array}{cc}{\Sigma }_{pp}& {\Sigma }_{pv}\\ {\Sigma }_{vp}& {\Sigma }_{vv}\end{array}\right]\end{array}$$
  接着，我们要通过查看当前$k-1$时刻状态来预测$k$时刻的状态。从原始预测中取每一点，将其移动到新的预测位置。如果原始预测正确，系统会移动到新位置。用矩阵$F_k$表示预测步骤：

图3 转换矩阵

  用矩阵来预测机器人下一刻的位置和速度：
$$\begin{array}{rl}{p}_k& =p_{k-1}+△t{v}_{k-1}\\ v_k& =v_{k-1}\end{array}$$

  转换成矩阵形式：
$$\begin{array}{rl}{\hat{x}}_k& =\left[\begin{array}{cc}1& △t\\ 0& 1\end{array}\right]{\hat{x}}_{k-1}\\ & =F_k{\hat{x}}_{k-1}\end{array}$$

  这是预测矩阵，给出机器人下一时刻的状态。但目前还未知协方差矩阵的更新，所以要引出下面等式：将分布中的每个点乘以矩阵$A$，协方差发生如下变化：
$$\begin{array}{rl}Cov\left(x\right)& =\Sigma \\ Cov\left(Ax\right)& =A\Sigma A^T\end{array}$$

  把该式与上述最佳估计${\hat{x}}_k$结合，可得：
$$\begin{array}{rl}{\hat{x}}_k& =F_k{\hat{x}}_{k-1}\\ P_k& =F_k{P}_{k-1}{F}_k^T\end{array}$$

1.外部因素

  除了位置与速度，外部因素也会对系统噪声影响，例如模拟火车运动，除了火车自驾系统，列车员可能也会手动调速。在机器人示例中，导航软件也可以发出停止指令。对于这些信息，我们把它作为一个向量${\vec{u}}_k$，纳入预测系统作为修正。

  假设电机速率设置和控制命令是已知的，我们知道机器人的预期加速度$a$。根据运动学基本定理，我们可得：
$$\begin{array}{rl}{p}_k& =p_{k-1}+△t{v}_{k-1}+\frac{1}{2}a△t^2\\ v_k& =v_{k-1}+a△t\end{array}$$

  转换为矩阵形式：
$$\begin{array}{rl}{\hat{x}}_k& =F_k{\hat{x}}_{k-1}+\left[\begin{array}{c}\frac{△t^2}{2}\\ △t\end{array}\right]a\\ & =F_k{\hat{x}}_{k-1}+B_k{\vec{u}}_k\end{array}$$

  $B_k$是控制矩阵，${\vec{u}}_k$是控制向量。如果外部环境异常简单，我们可以忽略这部分内容，但是如果添加外部因素后，模型的准确率还是上不去，这又是为什么呢？

2.外部不确定性

  当我们监控无人机时，它可能会受到风的影响；当我们跟踪轮式机器人时，它的轮胎可能会打滑，或者粗糙地面会降低它的移速。这些因素是难以掌握的，如果出现其中的任意一种情况，预测结果就难以保障。

  这要求我们在每个预测步骤后再加上一些新的不确定性，来模拟和可能存在的所有不确定性：

图4 外部不确定性

  如上图所示，加上外部不确定性后，${\hat{x}}_{k-1}$的每个预测状态都可能会移动到另一点，也就是蓝色的高斯分布会移动到橙色高斯分布的位置，并且具有协方差$Q_k$。换句话说，我们把这些不确定影响视为协方差$Q_k$的噪声。橙色的高斯分布拥有和原分布相同的均值，但协方差不同。在原式上加入$Q_k$：
$$\begin{array}{rl}{\hat{x}}_k& =F_k{\hat{x}}_{k-1}+B_k{\vec{u}}_k\\ P_k& =F_k{P}_{k-1}{F}_k^T+Q_k\end{array}$$

  这里，新的最佳估计是基于上一时刻最佳估计和已知外部因素校正后得到的预测。新的不确定性是基于上一时刻不确定性和外部环境不确定性得到的预测。现在，有了这些概念，可以把传感器数据输入其中。

3.通过量测来细化估计值

  如果存在多个传感器，它们一起提供有关系统状态的信息。传感器可以读取位置，可以读取速度，它能告诉我们关于状态的间接信息——它是状态下产生的一组读数。

图5 量测信息

  请注意，传感器读数的规模和状态的规模不一定相同，所以我们把传感器读数矩阵设为$H_k$。

图6 转换矩阵

  把这些分布转换为一般形式：
$$\begin{array}{rl}{\vec{\mu }}_{sensor}& =H_k{\hat{x}}_k\\ {\Sigma }_{sensor}& =H_k{P}_k{H}_k^T\end{array}$$

  卡尔曼滤波的一大优点是擅长处理传感器噪声。换句话说，由于种种因素，传感器采集的数据是不准确的，一个状态事实上可以产生多种读数。

图7 传感器存在噪声

  我们将这种不确定性（即传感器噪声）的协方差设为$R_k$，读数的分布均值设为$z_k$。现在我们得到两块高斯分布，一块围绕预测的均值，另一块围绕传感器读数。

  如果要生成靠谱预测，模型必须调和这两个信息。也就是说，对于任何可能的读数$\left(z_1,z_2\right)$，这两种方法预测的状态都有可能是准的，也都有可能是不准的。重点是我们怎么找到这两个准确率。

  最简单的方法是两者相乘，两块高斯分布相乘后，我们可以得到它们的重叠部分，这也是会出现最佳估计的区域。换个角度看，它看起来也符合高斯分布：

图8 两个高斯分布相乘

  事实证明，当两个高斯分布和它们各自的均值和协方差矩阵相乘时，会得到一个拥有独立均值和协方差矩阵的新高斯分布。最后剩下的问题就不难解决了：必须有一个公式来从旧的参数中获取这些新参数！

4.结合高斯

  让我们从一维看起，设均值为$\mu$，方差为${\sigma }^2$，一个标准一维高斯钟形曲线方程如下所示：
$$\mathcal{N}\left(x,\mu ,\sigma \right)=\frac{1}{\sigma \sqrt{2\pi }}{e}^{-\frac{{\left(x-\mu \right)}^2}{2{\sigma }^2}}$$

  若两个高斯相乘，结果是否还是高斯分布呢？

图9 融合后的高斯分布

  如图所示红色和绿色分别代表两个独立的高斯分布函数，蓝色为两个分布的乘积，从蓝色形状可以粗略看出乘积结果可能为一个幅值被压缩的高斯分布，期望在$\left[{\mu }_1,{\mu }_2\right]$之间。那真的是这样的吗？需要理论的推导来进行证明。
$$\begin{array}{rl}\mathcal{N}\left(x,\mu \prime ,\sigma \prime \right)& =\mathcal{N}\left(x,{\mu }_1,{\sigma }_1\right)\cdot \mathcal{N}\left(x,{\mu }_2,{\sigma }_2\right)\\ & =\frac{1}{{\sigma }_1\sqrt{2\pi }}{e}^{-\frac{{\left(x-{\mu }_1\right)}^2}{2{\sigma }_1^2}}\frac{1}{{\sigma }_2\sqrt{2\pi }}{e}^{-\frac{{\left(x-{\mu }_2\right)}^2}{2{\sigma }_2^2}}\\ & =\frac{1}{2\pi {\sigma }_1{\sigma }_2}{e}^{-\left[\frac{{\left(x-{\mu }_1\right)}^2}{2{\sigma }_1^2}+\frac{{\left(x-{\mu }_2\right)}^2}{2{\sigma }_2^2}\right]}\end{array}$$

  把指数部分单独提出：
$$\begin{array}{rl}\beta & =\frac{{\left(x-{\mu }_1\right)}^2}{2{\sigma }_1^2}+\frac{{\left(x-{\mu }_2\right)}^2}{2{\sigma }_2^2}\\ & =\frac{\left({\sigma }_1^2+{\sigma }_2^2\right)x^2-2\left({\mu }_2{\sigma }_1^2+{\mu }_1{\sigma }_2^2\right)x+\left({\mu }_2^2{\sigma }_1^2+{\mu }_1^2{\sigma }_2^2\right)}{2{\sigma }_1^2{\sigma }_2^2}\\ & =\frac{x^2-2\frac{{\mu }_2{\sigma }_1^2+{\mu }_1{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}x+\frac{{\mu }_2^2{\sigma }_1^2+{\mu }_1^2{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}}{\frac{2{\sigma }_1^2{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}}\\ & =\frac{{\left(x-\frac{{\mu }_2{\sigma }_1^2+{\mu }_1{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}\right)}^2+\frac{{\mu }_2^2{\sigma }_1^2+{\mu }_1^2{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}-{\left(\frac{{\mu }_2{\sigma }_1^2+{\mu }_1{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}\right)}^2}{\frac{2{\sigma }_1^2{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}}\\ & =\underset{\gamma }{\underbrace{\frac{{\left(x-\frac{{\mu }_2{\sigma }_1^2+{\mu }_1{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}\right)}^2}{\frac{2{\sigma }_1^2{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}}}}+\underset{\lambda }{\underbrace{\frac{\frac{{\mu }_2^2{\sigma }_1^2+{\mu }_1^2{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}-{\left(\frac{{\mu }_2{\sigma }_1^2+{\mu }_1{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}\right)}^2}{\frac{2{\sigma }_1^2{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}}}}\end{array}$$

  其中，$\gamma$为一个$\left(\mu \prime ,\sigma \prime \right)$的正态分布，$\lambda$是一个常数值。继续简化$\lambda$有：
$$\begin{array}{rl}\lambda & =\frac{\frac{{\mu }_2^2{\sigma }_1^2+{\mu }_1^2{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}-{\left(\frac{{\mu }_2{\sigma }_1^2+{\mu }_1{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}\right)}^2}{\frac{2{\sigma }_1^2{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}}\\ & =\frac{\left({\mu }_2^2{\sigma }_1^2+{\mu }_1^2{\sigma }_2^2\right)\left({\sigma }_1^2+{\sigma }_2^2\right)-{\left({\mu }_2{\sigma }_1^2+{\mu }_1{\sigma }_2^2\right)}^2}{2{\sigma }_1^2{\sigma }_2^2\left({\sigma }_1^2+{\sigma }_2^2\right)}\\ & =\frac{\left({\mu }_2^2{\sigma }_1^4+{\mu }_2^2{\sigma }_1^2{\sigma }_2^2+{\mu }_1^2{\sigma }_1^2{\sigma }_2^2+{\mu }_1^2{\sigma }_2^4\right)-\left({\mu }_2^2{\sigma }_1^4+2{\mu }_1{\mu }_2{\sigma }_1^2{\sigma }_2^2+{\mu }_1^2{\sigma }_2^4\right)}{2{\sigma }_1^2{\sigma }_2^2\left({\sigma }_1^2+{\sigma }_2^2\right)}\\ & =\frac{{\sigma }_1^2{\sigma }_2^2\left({\mu }_1^2+{\mu }_2^2-2{\mu }_1{\mu }_2\right)}{2{\sigma }_1^2{\sigma }_2^2\left({\sigma }_1^2+{\sigma }_2^2\right)}\\ & =\frac{{\left({\mu }_1-{\mu }_2\right)}^2}{2\left({\sigma }_1^2+{\sigma }_2^2\right)}\end{array}$$

  两个高斯分布相乘为：
$$\begin{array}{rl}\mathcal{N}\left(x,\mu \prime ,\sigma \prime \right)& =\mathcal{N}\left(x,{\mu }_1,{\sigma }_1\right)\cdot \mathcal{N}\left(x,{\mu }_2,{\sigma }_2\right)\\ & =\frac{1}{2\pi {\sigma }_1{\sigma }_2}{e}^{-\left(\gamma +\lambda \right)}\\ & =\frac{1}{2\pi {\sigma }_1{\sigma }_2}{e}^{-\gamma }\cdot e^{-\lambda }\\ & =\frac{1}{2\pi {\sigma }_1{\sigma }_2}{e}^{-\frac{{\left(x-\mu \prime \right)}^2}{2\sigma {\prime }^2}}\cdot e^{-\frac{{\left({\mu }_1-{\mu }_2\right)}^2}{2\left({\sigma }_1^2+{\sigma }_2^2\right)}}\end{array}$$

  其中，
$$\begin{array}{rl}\mu \prime & =\frac{{\mu }_2{\sigma }_1^2+{\mu }_1{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}={\mu }_1+\frac{{\sigma }_1^2\left({\mu }_2-{\mu }_1\right)}{{\sigma }_1^2+{\sigma }_2^2}\\ \sigma {\prime }^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}\end{array}$$

  用$k$简化一下：
$$\begin{array}{rl}k& =\frac{{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2}\\ \mu \prime & ={\mu }_1+k\left({\mu }_2-{\mu }_1\right)\\ \sigma {\prime }^2& ={\sigma }_1^2-k{\sigma }_1^2\end{array}$$

  以上是一维，如果是多维空间，把这个式子转成矩阵格式：
$$\begin{array}{rl}K& =\frac{{\Sigma }_1}{{\Sigma }_1+{\Sigma }_2}\\ \vec{\mu }\prime & ={\vec{\mu }}_1+K\left({\vec{\mu }}_2-{\vec{\mu }}_1\right)\\ \Sigma \prime & ={\Sigma }_1-K{\Sigma }_1\end{array}$$

  矩阵K就是我们说的卡尔曼增益。

5.结合在一起

  截止目前，我们有矩阵$\left({\mu }_1,{\Sigma }_1\right)=\left(H_k{\hat{x}}_k,H_k{P}_k{H}_k^T\right)$预测的分布，有用传感器读数预测的分布$\left({\mu }_2,{\Sigma }_2\right)=\left({\vec{z}}_k,R_k\right)$，把它们代入上节的矩阵等式中：
$$\begin{array}{rl}K& =\frac{H_k{P}_k{H}_k^T}{H_k{P}_k{H}_k^T+R_k}\\ H_k{\hat{x}}_k^{\prime }& =H_k{\hat{x}}_k+K\left({\vec{z}}_k-H_k{\hat{x}}_k\right)\\ H_k{P}_k^{\prime }{H}_k^T& =H_k{P}_k{H}_k^T-K{H}_k{P}_k{H}_k^T\end{array}$$

  简化上式有：
$$\begin{array}{rl}K\prime & =\frac{H_k^T{P}_k}{H_k{P}_k{H}_k^T+R_k}\\ {\hat{x}}_k^{\prime }& ={\hat{x}}_k+K\prime \left({\vec{z}}_k-H_k{\hat{x}}_k\right)\\ P_k^{\prime }& =\left(I-K\prime H_k\right)P_k\end{array}$$

  最后，${\hat{x}}_k^{\prime }$是我们的最佳估计值，我们可以把它继续放进去做下一轮进行预测。

四、卡尔曼滤波五大公式总结

  总结一下卡尔曼滤波的预测与更新的五个公式：

预测：
$$\begin{array}{rl}{\hat{x}}_{\mathrm{k}}& =F_{\mathrm{k}}{\hat{x}}_{\mathrm{k}-1}+B_{\mathrm{k}}{\vec{u}}_{\mathrm{k}}\\ P_k& =F_k{P}_{k-1}{F}_k^T+Q_k\end{array}$$

更新：
$$\begin{array}{rl}K\prime & =\frac{H_k^T{P}_k}{H_k{P}_k{H}_k^T+R_k}\\ {\hat{x}}_k^{\prime }& ={\hat{x}}_k+K\prime \left({\vec{z}}_k-H_k{\hat{x}}_k\right)\\ P_k^{\prime }& =\left(I-K\prime H_k\right)P_k\end{array}$$

  以上就是卡尔曼滤波完整推导，若文章中出现错误或者小伙伴对以上内容有所疑问，欢迎大家在评论区留言讨论，小政看到后会尽快回复大家！