前言
本文是对卡尔曼滤波学习的记录，主要参照了DR_CAN老师的视频进行学习。视频专栏链接：DR_CAN卡尔曼滤波视频专栏。虽然网上有很多卡尔曼滤波原理介绍的相关博客，但像DR_CAN老师讲解的如此详细的却不多，我也将自己跟随老师学习的记录下来。
本篇文章为卡尔曼滤波原理详解的第二篇，第一篇链接在此：卡尔曼滤波原理详解（一）
卡尔曼增益推导
继续紧接上一篇推导卡尔曼增益。
根据前文我们引入的例子及数据融合的思想，我们给出了一个系统的状态空间表达式来描述一个系统的动态响应
不考虑过程噪声，通过上述公式(1.13)所估算出来的 X k X_k Xk，我们记为先验估计值 X ^ k − \hat X_k^- X^k−。
则根据上文数据融合的思想，我们可以将后验估计值 X ^ k \hat X_k X^k写成：
其中 G ∈ [ 0 , 1 ] G\in[0,1] G∈[0,1] ，上述式(1.14)即是让我们在算出来的和测出来的数据之间，更倾向于相信哪个数据。一些书上有时候也将G写作 K k H K_kH KkH，则
那么现在的目标即是寻找合适的 K k K_k Kk，使得 X ^ k \hat X_k X^k→ X k X_k Xk的实际值。
令 e k = X k − X ^ k e_k=X_k-\hat X_k ek=Xk−X^k， e k e_k ek满足高斯分布，满足 P ( e k ) ∼ ( 0 , P ) P(e_k)\sim(0,P) P(ek)∼(0,P) 。问题转换为寻找合适的 K k K_k Kk，使得 e k e_k ek的方差最小，即 e k e_k ek的协方差矩阵最小？
又
其中 e k − e_k^- ek−示先验误差估计，则将(1.17)代入(1.16)，得到
其中：
由于 e k − e_k^- ek−与 v k T v_k^T vkT互不相关，且二者期望均为0，故
同样地， E [ K k v k ( I − K k H ) T e k − T ] = 0 E[K_kv_k(I-K_kH)^Te_{k}^{-T}]=0 E[Kkvk(I−KkH)Tek−T]=0，则 P k P_k Pk为：
其中 P k − P_k^- Pk−表示先验误差估计 e k − e_k^- ek−的协方差矩阵， R为测量噪声 v k v_k vk的方差。注意到 P k − P_k^- Pk−是对称矩阵，则 
。
则只需使 P k P_k Pk的迹最小，便可以使 e k e_k ek的方差最小。求式(1.21)的迹
类似前一篇文章数据融合的步骤，令 d t r ( P k ) d K k = 0 \frac{dtr(P_k)}{dK_k}=0 dKkdtr(Pk)=0，找寻其极值点。
求解，得：
所求式(1.24)即为卡尔曼增益。其中 P k − P_k^- Pk−是协方差矩阵，其转置等于本身。
至此，我们已知一系统
先验估计为：
后验估计为：
卡尔曼增益：
上述公式中 P k − P_k^- Pk−（先验误差协方差）未知。
又
将式(1.26)代入到(1.25)中，得到：
其中 e k − 1 e_{k-1} ek−1与 w k − 1 T w_{k-1}^T wk−1T互不相关，且期望均为0，因此 E ( A e k − 1 w k − 1 T ) = 0 E(Ae_{k-1}w_{k-1}^T)=0 E(Aek−1wk−1T)=0，同样地 E ( w k − 1 A T e k − 1 T ) = 0 E(w_{k-1}A^Te_{k-1}^T)=0 E(wk−1ATek−1T)=0，则
其中Q为测量噪声 w k w_k wk的方差。
总结
至此，我们便可以利用卡尔曼滤波进行估计。主要分为两步，分别是预测和校正。
后续校正还需要用到后验误差协方差矩阵以便更新下一次的信息。
其中后验协方差矩阵的计算结合式(1.21)和卡尔曼增益(1.24)求得，具体如下：
至此，卡尔曼滤波的五个公式全部推导完毕，如下表所示。
至此，卡尔曼滤波的原理讲解就记录到这里了，强烈建议观看DR_CAN老师的视频教程！