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

许多博主或简或复杂地介绍了卡尔曼滤波器，下面我用自己的理解再次理解：

师兄说KF没什么好说的，就那五个公式，用的时候在代码里找到自己的变量，带进去。但是我还是想弄清楚中间推导的过程


或者借用这个可爱的小例子，机器人需要知道它在森林里的确切位置来规划路线，跑来跑去，因为他只安装了它GPS，而GPS精度只有5米，如果他犯了一点错误，他可能会掉进悬崖。那么，如何保证他的移动更安全呢？GPS显然不够，我们可以给他带激光雷达，imu等待传感器共同约束他的定位。

假设机器人的状态是卡尔曼滤波器，它包含位置和速度，并服从高斯分布。每个变量都有一个平均值 μ，表示随机分布的中心(最有可能的状态)和方差。位置和速度可能不相关(另一个变量的值不能由一个变量估计)(速度快，说明位置移动多)，卡尔曼滤波只能用于相关，这正是他的目的:尽可能从包含不确定性的测量数据中提取更多信息！
它是协方差矩阵。矩阵中的每个元素都用来描述第一个和第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是卡尔曼增益。