首先分成两个Part吧：直观和公式。
预备知识
1. 协方差矩阵是对称矩阵，对角线上是各维度本身的方差，其他是不同维度间的协方差； 2. 贝叶斯公式：
3. 多维高斯分布：
4. 期望和协方差矩阵： 在 时，
直观部分
先来说一说直观的。借鉴了这篇的图文：https://courses.engr.illinois.edu/ece420/sp2017/UnderstandingKalmanFilter.pdf 对一个小车的状态进行估计，假设无外力作用，即保持初速度持续不断的运动。假设马尔科夫性，即 只与
有关，给出一个递推方程
。理论上可以通过这个方程求出任意时刻的状态，但是事实上会受到不确定因素的影响。 我们假设每个状态分量受到的不确定因素（噪声）服从正态分布，那么小车的状态分量也就服从正态分布，现在对小车进行状态估计。
上图是t=0时小车的位置服从的状态分布。 
通过公式预测出小车在t=1时刻位置的正态分布（如上图）。 因为加了噪声，所以不确定度增加了，分布也变宽了。这时为了避免不确定度带来的偏差，用传感器测量一次小车的位置，如下图深色分布： 
当然，传感器测量同样有不确定因素的干扰，这时，你该相信哪个呢？传感器也可能出错，估计方程也有偏差，这里就需要加权，卡尔曼滤波的K，也就是卡尔曼增益就是这个权重。这里捎带着提一下 后面会在公式部分进行推导，这里的
就是方程推算是一个后验概率，
和
都是传感器观测方程的参数。
加权后的结果就是图中绿色部分了。
公式推导部分
这里我使用的是视觉SLAM十四讲中的最大后验概率推导，可能比书上看的明朗些，因为高博喜欢不写中间过程或者不给出之前从哪得到过的结论，直接拿来用。 声明： 是后验概率，
是先验概率 状态估计和观测模型的方程：①
可以看出是线性方程。
先验与
的方程：②
，
的均值是0，故不加在
上了。 要求算的方程：
意义：后验是由先验信息
和其他信息
综合得出的
等式右侧都符合高斯分布，为啥子？因为噪声都是高斯分布的且是以0为均值，加上估计值当然是以估计值为均值的高斯分布了。 那么我们可以这样写：③ ，在②式中给出了
的均值、协方差矩阵的求法。 随后我们对③式中高斯分布的二次项进行计算： 高斯分布公式：（看到exp后面的了吧）
推导：
①这个式子后面我们会用到！ ②步将 这就是卡尔曼增益。 这样就有了
，可以注意到这里的卡尔曼增益是用了
算出来的，感觉有点转圈圈，就是要求的东西放到求解的式子里了好像没有移项。 这里我们使用刚才推导给出的①式对
继续推： 就有了：
根据SMW（Sherman-Morrison-Woodbury）恒等式：
上述的最后一个恒等式，可得： 。 好了现在要求的后验概率已经求出了协方差，还有均值要求解，对③式中高斯分布的一次项进行计算：
其中式①是将等号左右均求转置，由于协方差矩阵是对称矩阵，所以不变。式②使用了 和
。最终得到了
即如何递归的求解均值。 到这里公式部分就结束了。
