基于卡尔曼滤波的多传感器

追踪静止和移动的目标是自动驾驶技术领域最为需要的核心技术之一。来源于多种传感器的信号，包括rtk、摄像头、激光雷达、IMU等传感器组合的组合体来估计位置，速度，轨迹以及目标的种类，例如其他车辆和行人。

为什么我们需要这么多的传感器？

这是因为每种传感器提供了追踪物体所需要的不同精度和类型的信息，尤其是在不同天气条件下。比如，以激光雷达为基础的传感器能很好地解决位置的问题，但是在糟糕的天气条件下其精度和性能都会有很大程度的下降。

组合来自不同传感器信息的技术称之为传感器融合技术。之所以较早的讨论这个，是因为应用在传感器融合之上的算法必须处理短暂的，充满杂讯的输入，生成可靠的运动状态估计的概率。

将这些传感器融合需要一些方法，卡尔曼滤波是一种在不确定状况下组合多源信息得到所需状态最优估计的一种方法。

卡尔曼滤波

很多博主或简或繁的介绍过卡尔曼滤波，下面我用自己的理解再捋顺一遍：

师兄说KF没啥好讲的，就那5个公式，用的时候把自己的变量在代码中找到，带进去就行了。可是我还是想把中间推导的过程弄清楚


还是借用这个小可爱的例子，这个机器人在森林里需要知道它所在的确切位置才能规划路线、跑来跑去，由于他身上只装了GPS，而GPS的精度只有5米，那他稍微走错一点儿可能就会掉进悬崖，那么，如何让他的移动更安全有保障呢，只有GPS显然不够，我们可以再给他带上激光雷达、imu等传感器来共同约束他的定位。

卡尔曼滤波假设机器人的一个状态，里面包含位置和速度，并且服从高斯分布。每个变量都有一个均值 μ，表示随机分布的中心（最可能的状态），以及方差。位置和速度可能不相关（由一个变量无法估计出另一个变量的值）可能相关（速度很快，说明位置移动的也多），而只有相关，才能应用卡尔曼滤波，这也恰恰是他的目的：尽可能地在包含不确定性的测量数据中提取更多信息！
是协方差矩阵，矩阵中的每个元素都用来描述第i个和第j个状态变量之间的相关性，比如，我们上面的例子中只有两个状态变量，那么这是一个2*2的对称矩阵。

我们基于高斯分布来建立状态变量，所以在时刻 k 需要两个信息：最佳估计（即均值）其它地方常用 μ 表示），以及协方差矩阵

现在的任务需要明确一下：用当前状态（k-1 时刻）来预测下一状态（k 时刻）！也就是明确当前的最佳估计和协方差矩阵。
用矩阵Fk来表示这个预测过程：
由基本的物理公式x=vt，（先假设速度与上一时刻保持不变）得到下面的式子
现在就得到了当前时刻的最佳估计，还差协方差矩阵了。如果将矩阵里的每一个元素都乘A，那么得到的新的协方差矩阵如下：
结合方程（4）和（3）得到：
这样，最基本的最佳估计和协方差矩阵都得到了～

一般来说，我们还会给小机器人一些外部控制，比如在他即将跌入悬崖的时候在监控室里踩一脚刹车，或者来个油门助他一臂之力。
那么，用表示控制向量，这是我们外部给机器人的指令（不管是刹车还是油门）都存在一个期望的加速度 a。状态量变为以下：
转成矩阵形式：

Bk称为控制矩阵。

而在实际生活中，一定存在着各种干扰，比如小飞机受到风的干扰，轮式车、履带车会打滑，这时候，我们就不能对这些状态进行跟踪，所以在每次预测完成后，在当前预测出的状态量周围还有很多干扰状态量，他们分别产生一个个的小高斯分布，这些高斯分布拥有同样的均值，因为他们周围的干扰默认分布的是平均的，而协方差不同，因为协方差描述的是状态量之间的相关性，位置和速度都不同的情况下，协方差也就不同了，故建立一个新的协方差：。
我们通过简单地添加得到扩展的协方差，下面给出预测步骤的完整表达式：
新的最优估计是根据上一最优估计预测得到的，并加上已知外部控制量的修正。而新的不确定性由上一不确定性预测得到，并加上外部环境的干扰。
现在，我们就基本上完成了任务，找到了最优估计和协方差。

以上是根据速度和位置来模糊的估计出了机器人的动向，那再给他安上传感器，这些传感器也许可以测量出他的速度或位姿，前方障碍物，总之都间接的传达给我们了一些关于机器人的状态信息。
这些传感器可能是激光雷达，出来的是点云数据，imu（惯导）读出来的是x，y，z坐标，他们的数据类型和单位都不一样，所以我们用一个矩阵来表示这些传感器数据。这个矩阵对预测出来的状态给予一个“约束”，加入传感器后的均值和方差可以表示为：
可是，传感器也存在噪音，所以有时候不加传感器时预测的状态或许更接近真实值。

我们将这种不确定性（例如：传感器噪声）用协方差
表示，该分布的均值就是我们读取到的传感器数据，称之为
。
现在我们有了两个高斯分布，一个是在预测值附近，一个是在传感器读数附近。 我们必须在预测值和传感器测量值之间找到最优解。
那就把这两个高斯分布相乘，得到一个新的高斯分布。
经过归一化处理得到：
归一化参考：https://blog.csdn.net/chaosir1991/article/details/106910668/
相同部分作为k，简化式子：
化成矩阵的形式，矩阵K称为卡尔曼增益：

现在，我们手里有两个高斯分布，一个是在预测值附近加入传感器约束出来的高斯分布：

一个是传感器读数附近的高斯分布：

将这两个高斯分布放入（15）里，求出重叠部分：
由（14）可以得到K：
（16）分离出新的均值和协方差以及卡尔曼增益K：
OK 然后就把这个新的最优估计和协方差放到下一个预测中，不断迭代～
整个卡尔曼滤波的推导过程中，只需要记住：
明白他们代表的是什么含义。（7）说的是加入外部控制量和干扰以后，状态的预测结果。（18）说的是当**（7）融合了传感器的约束后的高斯分布，与传感器读数附近的高斯分布相融合**出来的最终结果，K是卡尔曼增益。