【简历完善】- SLAM - 第一篇：卡尔曼滤波的学习

场景：晚上你需要从自己的卧室去上厕所，你知道家里的布局，了解自己的步长，但是没有灯。你如何才能走到厕所呢？

一些术语

1. “预测” = “估计”。下面所说的预测和估计就是一回事。不同博客里面这两个词语大概意思也是一致的。
   比如，你摸黑走了10步，你“估计”你“大概”到了。
2. “观测”。当你摸黑走了10步，就要放水了。但你开了一下灯，想确定一下，发现自己还差一步。这就是观测的过程。
3. 总结：下面这张图（来源：link）中有四个变量都是要估计的。
   • x_k^ : 你估计自己大概率处于10步的位置。
   • Pk：你不太确定，但是估计距离马桶也就半步的误差，可以直接放水了。
   • x_k^’ : 你开灯，发现自己才走到大概9步的位置，不能放水。
   • Pk’ : 你比较确定自己的方位“大概”在9步的位置，比摸黑估计的 Pk 小多了。
4. 我们的卡尔曼滤波就是“预测”和“观测”的结合。思路是“预测”提供初值，观测进行优化。
   

一、直接目的：五个公式

1.1 第一个公式

这个k就是你的步数，xk^ 就是你的位置，Fk 就是你估计自己一步走多远。后面的那个 u，其实是加速度。因为论文中的xk不仅仅表示位置，还有速度。但是我们这里假设你摸黑走路是匀速的，先不管他。

1.2 第二个公式

一开始 k = 0 的时候，你是在卧室（原点，x=0）。但是你走着走着你越来越担心自己走偏了。这就反映在方差上，也就是第二个式子。因为你状态变换的时候乘了一个 Fk，所以你的方差会乘 Fk 的平方。Qk 有博客说是： 外部不确定性（你穿拖鞋可能打滑了，但是你不知道）。但我暂时不知道应该如何计算 Qk，先不管。

1.3 中间休息一会

上面的都是高中物理和高中概率论，而且都是你单纯依据“预测”进行的，接下来要开个灯进行“观测”，确定一下你的位置（就是优化一下）。接下来重头戏来了，第三个公式莫名来了个H？这是什么？

1.4 第三个公式

这个公式是真的不好理解。

1.4.1 转换矩阵的引入

转换矩阵Hk：将小车的状态数据转换到传感器数据层。（？H如何计算呢？）
有博客1 这样说的：

稍微解释一下，这里的

• u，可以理解为：当我走了10步的时候（x_k^=10），我应该看到马桶（u= 马桶）。这个H其实是你大脑把10步的信息转换成了马桶的图片。
• 方差：看到的马桶可大可小，不太确定。

1.4.2 开灯前后的对比

在开灯前，我们有以下分布：

• 分布1：我们的预测（均值 = 大概会看到马桶，方差 = 马桶大小可以变）。

在开灯后，我们有：

• 分布2：实际的分布（实际均值，实际方差）；我看了一下，这个实际均值和实际方差是指传感器的均值和方差。（这里我也不太清楚，如何依据一个照片来算这两个值，先放放）。

注意，上面两个分布都是完全知道的，而且描述的都是一个东西，博客1 采用的方法是：相乘！（意义是什么呢？）可以参考这张图。

比如你有一个数据（Z1,Z2），这个可能更靠近红色的中心（概率更大），也可能靠近绿色中心。要兼顾两种，那只能乘起来了（有点牵强，但是好像是有道理的，先不管，反正这样做可行）。

1.4.3 正式开始公式3的推导

经过上面的讨论，我们确定了目标：已知两个正太分布，求两个分布的乘积的分布。这个容易，大学概率论基础，我们直接给出新分布的均值和方差。

注意，我们把新的均值写成：u’ = 老的估计均值 + 更新量 的形式。然后就是重点了，把卡尔曼滤波前两个公式放到这个概率论的结果中：

上图中的三个公式就是公式 3 4 5！所以完结了！