卡尔曼滤波（Kalman Filter）原理浅析-数学理论推导-3
目录
-
- 前言
- 数学理论推导
- 误差协方差矩阵数学推导
- 结语
- 参考
前言
最近项目需求涉及到目标跟踪部分，准备从 DeepSORT 多目标跟踪算法入手。DeepSORT 中涉及的内容有点多，以前也就对其进行了简单的了解，但是真正去做发现总是存在这样或者那样的困惑，头疼，果然欠下的总该还的
一个个来吧，这个系列文章主要分享博主在学习 DeepSORT 中的 Kalman Filter 的相关知识，主要从两方面分享，一方面是数学理论推导，另一方面是比较通俗易懂的图例分析。
这篇文章主要分享从数学理论推导的方式去理解卡尔曼滤波器，包括误差协方差矩阵的详细推导
博主为初学者，欢迎交流讨论，若有问题欢迎各位看官批评指正！！！
数学理论推导
视频链接：【卡尔曼滤波器】_Kalman_Filter_全网最详细数学推导
注：博主也就把 DR_CAN 老师讲解的内容复述了一遍，强烈建议大家观看原视频！！！
误差协方差矩阵数学推导
这节内容将紧接着上一节我们会把卡尔曼滤波器剩下的公式推导出来，与上节内容相比，这节的计算量会小很多。
我们先来回忆一下上节内容，现在有这样的一个系统，用离散型的状态空间方程表达出来是下面这种形式：
X k = A X k − 1 + B U k − 1 + W k − 1 Z k = H X k + V k \begin{aligned} X_k &= AX_{k-1} + BU_{k-1} + W_{k-1} \\ Z_k &= HX_k + V_k \end{aligned} XkZk=AXk−1+BUk−1+Wk−1=HXk+Vk
其中 W W W 是过程噪声，它符合正态分布且期望是 0 0 0，协方差是 Q Q Q； V V V 是它的测量噪声，它也符合正态分布且期望是 0 0 0，协方差是 R R R
在上一节内容中，我们推导出来了下面的三个公式，分别是：
先验估计
X ^ k − = A X ^ k − 1 + B U k − 1 \hat{X}_k^- = A\hat{X}_{k-1} + BU_{k-1} X^k−=AX^k−1+BUk−1
后验估计
X ^ k = X ^ k − + K k ( Z k − H X ^ k − ) \hat{X}_k = \hat{X}_k^- + K_k(Z_k - H\hat{X}_k^-) X^k=X^k−+Kk(Zk−HX^k−)
卡尔曼增益
K k = P k − H T H P k − H T + R K_k = \frac{P_k^-H^T}{HP_k^-H^T+R} Kk=HPk−HT+RPk−HT
其中后验估计公式是一个数据融合的概念，选取不同的 K k K_k Kk 值，它就会趋向于先验结果或者测量结果
现在来分析一下这三个式子，在先验估计公式中， A A A 是已知的， X ^ k − 1 − \hat{X}_{k-1}^- X^k−1− 是上一次的结果也是知道的， B U k − 1 BU_{k-1} BUk−1 是我们的输入也是已知的，所以先验估计 X ^ k \hat{X}_k X^k 就是已知的。在后验估计公式中，因为先验估计 X ^ k − \hat{X}_k^- X^k− 是已知的，然后 Z k Z_k Zk 是已知的， H H H 是已知的，所以说如何 K k K_k Kk 是已知的就能够计算出后验估计 X ^ k \hat{X}_k X^k 从而解决这个问题了
然后我们看到 K k K_k Kk 的第一项 P k − P_k^- Pk− 是我们之前没有表达出来的，所以这节就是要去把 P k − P_k^- Pk− 给它找出来
那 P k − P_k^- Pk− 等于什么呢？
根据定义 P k − = [ e k − , e k − T ] P_k^- = [e_k^-,e_k^{-T}] Pk−=[ek−,ek−T]，那 e k − e_k^- ek− 等于什么呢？它等于真实值 X k X_k Xk 减去先验估计值 X ^ k − \hat{X}_k^- X^k−，其计算如下式：
e k − = X k − X ^ k − = ( A X k − 1 + B U k − 1 + W k − 1 ) − ( A X ^ k − 1 + B U k − 1 ) = A ( X k − 1 − X ^ k − 1 ) + W k − 1 = A e k − 1 + W k − 1 \begin{aligned} e_k^- &= X_k - \hat{X}_k^- \\ &= (AX_{k-1} + BU_{k-1} + W_{k-1}) - (A\hat{X}_{k-1}+BU_{k-1}) \\ &= A(X_{k-1}-\hat{X}_{k-1}) + W_{k-1} \\ &= Ae_{k-1} + W_{k-1} \end{aligned} ek−=Xk−X^k−=(AXk−1+BUk−1+Wk−1)−(AX^k−1+BUk−1)=A(Xk−1−X^k−1)+Wk−1=Aek−1+Wk−1
把上式代入到 P k − P_k^- Pk− 的计算中有：
P k − = E [ e k − , e k − T ] = E [ ( A e k − 1 + W k − 1 ) ( A e k − 1 + W k − 1 ) T ] = E [ ( A e k − 1 + W k − 1 ) ( e k − 1 T A T + W k − 1 T ) ] = E [ A e k − 1 e k − 1 T A T + A e k − 1 W k − 1 T + W k − 1 e k − 1 T A T + W k − 1 W k − 1 T ] = E [ A e k − 1 e k − 1 T A T ] + E [ A e k − 1 W k − 1 T ] + E [ W k − 1 e k − 1 T A T ] + E [ W k − 1 W k − 1 T ] \begin{aligned} P_k^- &= E[e_k^-,e_k^{-T}] \\ &= E[(Ae_{k-1} + W_{k-1})(Ae_{k-1} + W_{k-1})^T] \\ &= E[(Ae_{k-1} + W_{k-1})(e_{k-1}^TA^T+W_{k-1}^T)] \\ &= E[Ae_{k-1}e_{k-1}^TA^T+Ae_{k-1}W_{k-1}^T+W_{k-1}e_{k-1}^TA^T+W_{k-1}W_{k-1}^T] \\ &= E[Ae_{k-1}e_{k-1}^TA^T]+E[Ae_{k-1}W_{k-1}^T]+E[W_{k-1}e_{k-1}^TA^T]+E[W_{k-1}W_{k-1}^T] \\ \end{aligned} Pk−=E[ek−,ek−T]=E[(Aek−1+Wk−1)(Aek−1+Wk−1)T]=E[(Aek−1+Wk−1)(ek−1TAT+Wk−1T)]=E[Aek−1ek−1TAT+Aek−1Wk−1T+Wk−1ek−1TAT+Wk−1Wk−1T]=E[Aek−1ek−1TAT]+E[Aek−1Wk−1T]+E[Wk−1ek−1TAT]+E[Wk−1Wk−1T]
这个时候我们来看第二项，这里面的 e k − 1 e_{k-1} ek−1 和 W k − 1 W_{k-1} Wk−1 是相互独立的，为什么呢？
我们回过头去看最开始的式子 e k − 1 = X k − 1 − X ^ k − 1 e_{k-1} = X_{k-1} - \hat{X}_{k-1} ek−1=Xk−1−X^k−1，就是上一次的真实值减去上一次的估计值，而由 X k = A X k − 1 + B U k − 1 + W k − 1 X_k = AX_{k-1} + BU_{k-1} + W_{k-1} Xk=AXk−1+BUk−1+Wk−1 可知 W k − 1 W_{k-1} Wk−1 是作用在 X k X_k Xk 上的，是作用在这一次上的。 e k − 1 e_{k-1} ek−1 是上一次的， W k − 1 W_{k-1} Wk−1 是这一次的，所以这两个没有相互关系，它们是相互独立的。
而如果它们相互独立的话，它们两个乘积的期望就等于它们期望的乘积，即 E [ e k − 1 W k − 1 T ] = E [ e k − 1 ] E [ W k − 1 T ] E[e_{k-1}W_{k-1}^T]=E[e_{k-1}]E[W_{k-1}^T] E[ek−1Wk−1T]=E[ek−1]E[Wk−1T]，然后我们知道这两个的期望都分别等于零，所以这一项就等于零了，同理第三项也等于零，因此我们就只剩下第一项和最后一项了
我们继续来推导 P k − P_k^- Pk−，如下所示：
P k − = E [ e k − , e k − T ] = E [ ( A e k − 1 + W k − 1 ) ( A e k − 1 + W k − 1 ) T ] = E [ ( A e k − 1 + W k − 1 ) ( e k − 1 T A T + W k − 1 T ) ] = E [ A e k − 1 e k − 1 T A T + A e k − 1 W k − 1 T + W k − 1 e k − 1 T A T + W k − 1 W k − 1 T ] = E [ A e k − 1 e k − 1 T A T ] + E [ A e k − 1 W k − 1 T ] + E [ W k − 1 e k − 1 T A T ] + E [ W k − 1 W k − 1 T ] = E [ A e k − 1 e k − 1 T A T ] + E [ W k − 1 W k − 1 T ] = A E [ e k − 1 e k − 1 T ] A T + E [ W k − 1 W k − 1 T ] = A P k − 1 A T + Q \begin{aligned} P_k^- &= E[e_k^-,e_k^{-T}] \\ &= E[(Ae_{k-1} + W_{k-1})(Ae_{k-1} + W_{k-1})^T] \\ &= E[(Ae_{k-1} + W_{k-1})(e_{k-1}^TA^T+W_{k-1}^T)] \\ &= E[Ae_{k-1}e_{k-1}^TA^T+Ae_{k-1}W_{k-1}^T+W_{k-1}e_{k-1}^TA^T+W_{k-1}W_{k-1}^T] \\ &= E[Ae_{k-1}e_{k-1}^TA^T]+E[Ae_{k-1}W_{k-1}^T]+E[W_{k-1}e_{k-1}^TA^T]+E[W_{k-1}W_{k-1}^T] \\ &= E[Ae_{k-1}e_{k-1}^TA^T] + E[W_{k-1}W_{k-1}^T] \\ &= AE[e_{k-1}e_{k-1}^T]A^T + E[W_{k-1}W_{k-1}^T] \\ &= AP_{k-1}A^T + Q \end{aligned} Pk−=E[ek−,ek−T]=E[(Aek−1+Wk−1)(Aek−1+Wk−1)T]=E[(Aek−1+Wk−1)(ek−1TAT+Wk−1T)]=E[Aek−1ek−1TAT+Aek−1Wk−1T+Wk−1ek−1TAT+Wk−1Wk−1T]=E[Aek−1ek−1TAT]+E[Aek−1Wk−1T]+E[Wk−1ek−1TAT]+E[Wk−1Wk−1T]=E[Aek−1ek−1TAT]+E[Wk−1Wk−1T]=AE[ek−1ek−1T]AT+E[Wk−1Wk−1T]=APk−1AT+Q
有了上面这个式子，我们就可以利用卡尔曼滤波器来估计状态变量的值了，它分为两个步骤，一个是预测，一个是校正。
在预测当中有两个部分，一个是先求出它的先验估计：
X ^ k − = A X ^ k − 1 − + B U k − 1 (1) \hat{X}_k^- = A\hat{X}_{k-1}^- + BU_{k-1} \tag{1} X^k−=AX^k−1−+BUk−1(1)
然后再去求它的先验误差协方差矩阵：
P k − = A P k − 1 A T + Q (2) P_k^- = AP_{k-1}A^T + Q \tag{2} Pk−=APk−1AT+Q(2)
在校正这里面，首先计算它的卡尔曼增益：
K k = P k − H T H P k − H T + R (3) K_k = \frac{P_k^-H^T}{HP_k^-H^T+R} \tag{3} Kk=HPk−HT+RPk−HT(3)
然后求它的后验估计：
X ^ k = X ^ k − + K k ( Z k − H X ^ k − ) (4) \color{red} \hat{X}_k \color{black} = \hat{X}_k^-+K_k(Z_k-H\hat{X}_k^-) \tag{4} X^k=X^k−+Kk(Zk−HX^k−)(4)
这也是我们需要得到的这个目标，就是我们的估计值 X ^ k \hat{X}_k X^k。有了这四步我们就可以得到一个最优的估计值了，就是这个 X ^ k \hat{X}_k X^k，而且在上一节内容当中我们还证明了它为什么是最优的，有疑问的看官可以参考上一节内容
我们有注意到在计算先验误差协方差矩阵 P k − P_k^- Pk− 的时候，我们会用到上一次的误差协方差矩阵 P k − 1 P_{k-1} Pk−1，所以说为了给下一次做准备，我们在校正之后还要更新一些后验的误差协方差矩阵 P k P_k Pk，这个 P k P_k Pk 将会在下一次的预测当中使用到，所以这里我们要把它更新一下。
那 P k P_k Pk 等于多少呢？
在上节内容中我们算出来了 P k P_k Pk 等于下面这一大串：
P k = P k − − K k H P k − − P k − H T K k T + K k H P k − H T K k T + K k R K k T P_k = P_k^--K_kHP_k^--P_k^-H^TK_k^T+K_kHP_k^-H^TK_k^T+K_kRK_k^T Pk=Pk−−KkHPk−−Pk−HTKkT+KkHPk−HTKkT+KkRKkT
然后我们可以把最后两项合并化简有：
P k = P k − − K k H P k − − P k − H T K k T + K k H P k − H T K k T + K k R K k T = P k − − K k H P k − − P k − H T K k T + K k ( H P k − H T + R ) K k T = P k − − K k H P k − − P k − H T K k T + P k − H T H P k − H T + R ( H P k − H T + R ) K k T = P k − − K k H P k − − P k − H T K k T + P k − H T K k T = P k − − K k H P k − = ( I − K k H ) P k − \begin{aligned} P_k &= P_k^--K_kHP_k^--P_k^-H^TK_k^T+K_kHP_k^-H^TK_k^T+K_kRK_k^T \\ &= P_k^--K_kHP_k^--P_k^-H^TK_k^T+\color{blue}K_k(HP_k^-H^T+R)K_k^T \\ &= P_k^--K_kHP_k^--P_k^-H^TK_k^T+\color{blue}\frac{P_k^-H^T}{HP_k^-H^T+R}(HP_k^-H^T+R)K_k^T \\ &= P_k^--K_kHP_k^--P_k^-H^TK_k^T+\color{blue}P_k^-H^TK_k^T \\ &= P_k^--K_kHP_k^- \\ &= (I-K_kH)P_k^- \end{aligned} Pk=Pk−−KkHPk−−Pk−HTKkT+KkHPk−HTKkT+KkRKkT=Pk−−KkHPk−−Pk−HTKkT+Kk(HPk−HT+R)KkT=Pk−−KkHPk−−Pk−HTKkT+HPk−HT+RPk−HT(HPk−HT+R)KkT=Pk−−KkHPk−−Pk−HTKkT+Pk−HTKkT=Pk−−KkHPk−=(I−KkH)Pk−
即 P k P_k Pk 的计算如下：
P k = ( I − K k H ) P k − (5) P_k = (I-K_kH)P_k^- \tag{5} Pk=(I−KkH)Pk−(5)
以上就是卡尔曼滤波器的的五大公式了，我们把它们放在一起
X ^ k − = A X ^ k − 1 − + B U k − 1 P k − = A P k − 1 A T + Q K k = P k − H T H P k − H T + R X ^ k = X ^ k − + K k ( Z k − H X ^ k − ) P k = ( I − K k H ) P k − \begin{aligned} \hat{X}_k^- &= A\hat{X}_{k-1}^- + BU_{k-1} \\ P_k^- &= AP_{k-1}A^T + Q \\ K_k &= \frac{P_k^-H^T}{HP_k^-H^T+R} \\ \color{red} \hat{X}_k \color{black} &= \hat{X}_k^-+K_k(Z_k-H\hat{X}_k^-) \\ P_k &= (I-K_kH)P_k^- \end{aligned} X^k−Pk−KkX^kPk=AX^k−1−+BUk−1=APk−1AT+Q=HPk−HT+RPk−HT=X^k−+Kk(Zk−HX^k−)=(I−KkH)Pk−
这就是完整的卡尔曼滤波器的五个公式，我们用了整整两节内容非常详细的进行了推导，希望大家可以掌握。
然后我们再来看下，这里面它每一次的预测都会用到上一次的结果，就这样一次次的往回推直到推到了第 0 次，也就是最开始的时候，因此我们需要给它赋予一个初值，分别是 X ^ 0 \hat{X}_0 X^0 和 P 0 P_0 P0，关于这个初值的问题，我们会在后面的小节中进行讨论。在之后的文章中，我们会通过例子来给大家分析一下卡尔曼滤波是怎么使用的。
结语
本篇博客详细推导了误差协方差矩阵，至此卡尔曼滤波器的五大公式就齐全了，这个还是需要各位多动手自己推导推导。卡尔曼滤波器的数学理论推导到这里就结束了，下篇文章我们将会分享关于卡尔曼滤波器应用的实例和扩展卡尔曼滤波器相关的知识，敬请期待
这里顺便吐槽下 CSDN，本来博主是想把数学理论推导全放在一篇文章的，但是发布时 CSDN 总显示正文字数太多，请分成几篇文章发布，无奈只能分开发布，给大家带来的困扰敬请谅解
参考
- 常用的MarkDown颜色 笔记
- 【卡尔曼滤波器】_Kalman_Filter_全网最详细数学推导【博主强烈推荐】