继续感谢Barfoot教授与高翔博士及小伙伴们

这篇算是大通稿了，写卡尔曼和讲过卡尔曼的资料，文章均不及其数。要想写出新意来，感觉还是从多传感器融合融态的方向来写大白话比较好，这篇整体偏向本科生和初学者。

基于这个思路，先列一下运动方程与观测方程(来自于机器人学中的状态估计，下标不会弄截图了)：

这2个方程可以说是机器人状态估计中的核心了，整体以及变量对应的含义都有了，注意除了vk是确定性变量外(在机器人系统中这个主要是IMU或者轮速计给出的)，其他都是随机变量。

2个噪声假设和初始状态不相关，自身全过程互不相关。

注意在这个位置中一定要牢记：Ak是观测矩阵，Ck是转移矩阵 

除此之外初始状态x0对应的协方差矩阵为P0 (通常先验和后验状态是有区别的，一般上标ˇ代表初始状态/先验，^代表状态估计结果或后验)

输入的测量值噪声的协方差矩阵Qk，观测变量的协方差矩阵Rk

卡尔曼滤波的整体推导看似非常复杂，很多初学者容易在这卡很久，接下来用大白话提出关键点来表述，但是建议大家还是要认真花时间看推导过程。

1.第一个关键点在提升形式z(先验+测量+观测)和x(对应的状态估计)，如果将对应变量(如一堆采样，一堆测量)列为列向量形式，则称为提升形式，向量与矩阵的运算是基础就不赘述了。

2.用Ak和Ck组合出H矩阵，用P0ˇ，Qk，Rk组出W矩阵，这一步非常关键(见机器人学中状态估计P37)

3.需要充分理解变量x二次形式，这是导出正规方程/增量方程的关键。

(H转置*W逆*H)x^=H转置*W逆z

正定方程要在机器人状态估计中经常用于反解残差r与Jacobian（这个看不懂没关系反正后面会讲）

4.H转置*W逆*H--->这个东西是个三对角阵，此处如果有兴趣的同学可以去研究能观性部分，有详细的推导过程，还会涉及到能观性矩阵，因为是大成熟经典卡尔曼，略过亦可。针对这种三对角矩阵会有一种非常经典的矩阵分解解法：cholesky，其本质是节约开销，将此矩阵分解为L*L的转置，L矩阵是下三角矩阵(其实上三角也可以，后续推导方向反过来)。最终就会有如下的形式：

根据第3点提到的正规/增量方程

H转置*W逆*H=L*L转置

形成前向迭代：Ld=H转置*W逆z 根据L的特殊结构，可从第一行L0开始轻易解出L来，再解出d

后向迭代：L转置*x=d 代入d和L轻易解出x

整个卡尔曼滤波就是利用正规/增量方程中H转置*W逆*H的特殊三对角块稀疏结构来完成的。5个前向迭代可理解为卡尔曼滤波。

5.卡尔曼滤波推导除了cholesky外还会涉及RTS方法用到的SMW恒等式，这个恒等式的来源在P22，就是利用标准可逆矩阵的分解特性：可分解为L(下三角矩)D(对角线矩)U(上三角矩)与UDL形式，分开分解后再将矩阵块一一对应就出了SMW恒等式，非常清晰。

cholesky卡尔曼形式：

RTS的卡尔曼形式

 经典卡尔曼形式

最后这张图的上半部分是最重要，非常直观和清晰地体现了卡尔曼进行状态估计的工作状态，其中需要同学们认真核对其中每一个小矩阵块表述的含义。同时需要对比他们之间形式的区别和差异，相对重要的是卡尔曼增益K的形式，以及红框框出来的更新量，这2个最好背下来。通常牢记来源和形式即可，详细推导可以看Barfoot教授这本书的译本。

因为是白话版解释，我只将推导过程中最关键的部分进行了描述，同样在这里机器人状态估计的世界发生了剧烈分叉，一方面是从KF走向了EKF，IEKF, 无迹卡尔曼，MSCKF，另一方面走向了BA与滑动窗口滤波法SWF，世界在此分叉，后续对应的说明如果工作涉及再描述吧。

但无论如何，卡尔曼滤波，正规/增量方程，cholesky分解，LDU/UDL与SMW恒等式，这些都是机器人状态估计中核心的核心。感谢为此付出的科学家与工程师们。