卡尔曼滤波（Kalman Filter）原理浅析-数学理论推导-2
目录
-
- 前言
- 数学理论推导
- 卡尔曼增益超详细数学推导
- 结语
- 参考
前言
最近项目需求涉及到目标跟踪部分，准备从 DeepSORT 多目标跟踪算法入手。DeepSORT 中涉及的内容有点多，以前也就对其进行了简单的了解，但是真正去做发现总是存在这样或者那样的困惑，头疼，果然欠下的总该还的
一个个来吧，这个系列文章主要分享博主在学习 DeepSORT 中的 Kalman Filter 的相关知识，主要从两方面分享，一方面是数学理论推导，另一方面是比较通俗易懂的图例分析。
这篇文章主要分享从数学理论推导的方式去理解卡尔曼滤波器，包括卡尔曼增益的详细推导
博主为初学者，欢迎交流讨论，若有问题欢迎各位看官批评指正！！！
数学理论推导
视频链接：【卡尔曼滤波器】_Kalman_Filter_全网最详细数学推导
注：博主也就把 DR_CAN 老师讲解的内容复述了一遍，强烈建议大家观看原视频！！！
卡尔曼增益超详细数学推导
这节我们继续来讨论卡尔曼滤波
在这节我们将一步一步的去详细推导这个 Kalman Gain，也就是说一下这个卡尔曼增益是怎么来的。所以这节是纯数学推导，计算量会非常的大，希望各位看官也能动动手一起来推导，下面我们开始。
在上节内容中的最后部分，我们给出了这个状态空间方程的表达形式，用这种状态空间方程的形式来描述一个系统的动态响应：
X k = A X k − 1 + B U k + W k − 1 Z k = H X k + V k \begin{aligned} X_k &= AX_{k-1}+BU_{k}+W_{k-1} \\ Z_k &= HX_k+V_{k} \end{aligned} XkZk=AXk−1+BUk+Wk−1=HXk+Vk
这里面 X k X_k Xk 就是它的状态量， A A A 是它的状态矩阵， B B B 是控制矩阵， U k U_{k} Uk 是控制，然后 W k − 1 W_{k-1} Wk−1 代表它的过程噪声，这部分内容我们是不可测的，也是我们没有办法去掌握的一部分内容，因为它是不确定性的一个表现。但是在自然界中一般这种噪声我们可以假设它符合正态分布，它的概率分布如下：
P ( W ) ∼ ( 0 , Q ) P(W) \sim (0,Q) P(W)∼(0,Q)
这里面 0 0 0 代表了它的期望，然后这个 Q Q Q 是它的协方差矩阵。如何去求这个 Q Q Q 呢？
其实 Q = E [ W W T ] Q = E[WW^T] Q=E[WWT]，其中 E E E 代表了期望，那这个公式怎么来的呢？我们用一个简单的例子来说明一下。
假设我们现在有两项，即 X = [ x 1 x 2 ] X =\begin{bmatrix}x_1\\x_2\end{bmatrix} X=[x1x2]，那它的过程噪声自然而然就是 w 1 w_1 w1 w 2 w2 w2，即 W = [ w 1 w 2 ] W =\begin{bmatrix}w_1\\w_2\end{bmatrix} W=[w1w2]
我们可以将 Q = E [ W W T ] Q=E[WW^T] Q=E[WWT] 展开为：
Q = E [ [ w 1 w 2 ] [ w 1 w 2 ] ] = E [ [ w 1 2 w 1 w 2 w 2 w 1 w 2 2 ] ] Q=E\left[\begin{bmatrix}w_1\\w_2\end{bmatrix}\begin{bmatrix}w_1&w_2\end{bmatrix}\right]=E\left[\begin{bmatrix}w_1^2&w_1w_2\\w_2w_1&w_2^2\end{bmatrix}\right] Q=E[[w1w2][w1w2]]=E[[w12w2w1w1w2w22]]
现在我们可以通过单独考虑每个元素来计算期望值。所以我们得到：
Q = [ E [ w 1 2 ] E [ w 1 w 2 ] E [ w 2 w 1 ] E [ w 2 2 ] ] Q=\begin{bmatrix}E[w_1^2]&E[w_1w_2]\\E[w_2w_1]&E[w_2^2]\end{bmatrix} Q=[E[w12]E[w2w1]E[w1w2]E[w22]]
我们知道协方差定义为：
C o v ( w 1 , w 2 ) = E [ w 1 w 2 ] − E [ w 1 ] E [ w 2 ] \mathrm{Cov}(w_1,w_2)=E[w_1w_2]-E[w_1]E[w_2] Cov(w1,w2)=E[w1w2]−E[w1]E[w2]
由于 w 1 w_1 w1 和 w 2 w_2 w2 的均值为 0 即 E [ w 1 ] = E [ w 2 ] = 0 E[w_1]=E[w_2]=0 E[w1]=E[w2]=0，这就简化了表达式：
C o v ( w 1 , w 2 ) = E [ w 1 w 2 ] \mathrm{Cov}(w_1,w_2)=E[w_1w_2] Cov(w1,w2)=E[w1w2]
这样我们可以将协方差写入矩阵的非对角元素中。对于对角元素，我们有方差：
Var ( w 1 ) = E [ w 1 2 ] − E [ w 1 ] 2 = E [ w 1 2 ] \operatorname{Var}(w_1)=E[w_1^2]-E[w_1]^2=E[w_1^2] Var(w1)=E[w12]−E[w1]2=E[w12]
所以我们得到：
Q = [ E [ w 1 2 ] C o v ( w 1 , w 2 ) C o v ( w 2 , w 1 ) E [ w 2 2 ] ] = [ V a r ( w 1 ) C o v ( w 1 , w 2 ) C o v ( w 2 , w 1 ) V a r ( w 2 ) ] Q=\begin{bmatrix}E[w_1^2]&\mathrm{Cov}(w_1,w_2)\\\mathrm{Cov}(w_2,w_1)&E[w_2^2]\end{bmatrix}=\begin{bmatrix}\mathrm{Var}(w_1)&\mathrm{Cov}(w_1,w_2)\\\mathrm{Cov}(w_2,w_1)&\mathrm{Var}(w_2)\end{bmatrix} Q=[E[w12]Cov(w2,w1)Cov(w1,w2)E[w22]]=[Var(w1)Cov(w2,w1)Cov(w1,w2)Var(w2)]
上面就是协方差矩阵 Q Q Q 的计算，它的对角线分别代表了 w 1 w_1 w1 的方差和 w 2 w_2 w2 的方差，它的另外两个值就代表了 w 1 w_1 w1 和 w 2 w_2 w2 的协方差。用这样一个协方差矩阵就可以表现出来这个过程噪声的方差还可以表现出来 w 1 w_1 w1 和 w 2 w_2 w2 之间的关系。
同样我们的测量状态空间方程中， V k V_k Vk 是测量噪声，它在我们的卡尔曼滤波里面也同样认为符合正态分布，它的概率分布如下：
P ( V ) ∼ ( 0 , R ) P(V) \sim (0, R) P(V)∼(0,R)
R R R 是它的协方差矩阵，具体计算还是跟上面一样，这边就不展开讲解了
我们现在来分析一下这个式子， X k X_k Xk 和 Z k Z_k Zk 都是现实当中的一种情况，但是我们在建模的过程当中 W k − 1 W_{k-1} Wk−1 和 V k V_k Vk 是噪声，没办法去建模。所以你掌握的也就是前面的部分即 A X k − 1 + B U k − 1 AX_{k-1}+BU_{k-1} AXk−1+BUk−1，这是你真真切切知道的东西，也就是说如果我想想去求 X k X_k Xk 的值，噪声我们是不知道的，我们只掌握了前面部分即 X k = A X k − 1 + B U k − 1 X_k = AX_{k-1} + BU_{k-1} Xk=AXk−1+BUk−1，但实际上这个表达式是不完整的，通过对比可知我们少了过程噪声这一项，所以它不是一个完整的值，它只是一个估计值，即表达式应该写成 X ^ k = A X k − 1 + B U k − 1 \hat{X}_k = AX_{k-1} + BU_{k-1} X^k=AXk−1+BUk−1 这种形式，其中 X ^ k \hat{X}_k X^k 表示的是估计值而不是实际值。
而且还有一点就是因为我们没有做任何的处理，我们只是根据之前的式子得出来的，因此我们需要在 X ^ k \hat{X}_k X^k 这块加上一个小小的 - 号，代表它叫先验估计，即 X ^ k − \hat{X}_k^- X^k−。也就是说我们直接通过状态空间方程然后去掉过程噪声计算出来的结果我们把它叫做先验估计值。
然后 X k − 1 X_{k-1} Xk−1 是上一时刻的状态值，这个也是没办法得到的，你只能根据上一次的估计结果 X ^ k − 1 \hat{X}_{k-1} X^k−1 来判断，所以得出我们的第一个公式：
X ^ k − = A X ^ k − 1 + B U k − 1 (1) \hat{X}_k^- = A\hat{X}_{k-1} + BU_{k-1} \tag1 X^k−=AX^k−1+BUk−1(1)
然后我们再来看下另外一项测出来的结果，测量结果 Z k Z_k Zk 我们是知道的，通过它我们可以得到 X ^ k m e a = H − Z k \hat{X}_{kmea} = H^{-}Z_k X^kmea=H−Zk
现在你可以发现我们有了两个结果，一个是 X ^ k − \hat{X}_k^- X^k− 先验结果，一个是 X ^ k m e a \hat{X}_{kmea} X^kmea 测出来的结果，我们可以把 X ^ k − \hat{X}_k^- X^k− 理解成算出来的结果， X ^ k m e a \hat{X}_{kmea} X^kmea 理解成测出来的结果。但是我们发现不管是算出来的还是测出来的都不具备噪声这一项，因为如果你可以完整的建模这个噪声的话，那么也就不存在卡尔曼滤波器了，也就不存在这个观测器的这个作用了，因为你直接可以得出结果来。但是正是因为有了这些噪声，导致我们得出来的结果都是不准确的。所以这个时候，卡尔曼滤波器的作用就要体现出来了。
我们如何通过两个不太准确的结果来得出一个准确的结果呢？
大家可以回忆一下上节中提到的数据融合的这个概念，这里面我们就可以用同样的方法。我们假设新的估计值（也叫做后验）计算如下：
X ^ k = X ^ k − + G ( H − Z k − X ^ k − ) \hat{X}_k = \hat{X}_k^- + G(H^-Z_k-\hat{X}_k^-) X^k=X^k−+G(H−Zk−X^k−)
当 G = 0 G=0 G=0 时， X ^ k = X ^ k − \hat{X}_k = \hat{X}_k^- X^k=X^k−；当 G = 1 G=1 G=1 时， X ^ k = H − Z k \hat{X}_k = H^-Z_k X^k=H−Zk
也就是说当 G → 0 G \to 0 G→0 的时候它更相信这个算出来的结果，当 G → 1 G \to 1 G→1 的时候它更相信这个测出来的结果。但是在一般的教科书上，我们看到的不是这样的一种形式，它用了一个很简单的变换即教科书上 G = K k H G = K_kH G=KkH 代入上式有：
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 ∈ [ 0 , H − ] K_k \in [0,H^{-}] Kk∈[0,H−] 而不再属于 0 ∼ 1 0 \sim 1 0∼1 的范围之间，同样当 K k = 0 K_k=0 Kk=0 时， X ^ k = X ^ k − \hat{X}_k = \hat{X}_k^- X^k=X^k−；当 K k = H − K_k=H^- Kk=H− 时， X ^ k = H − Z k \hat{X}_k = H^-Z_k X^k=H−Zk
即 K k = 0 K_k=0 Kk=0 时最终的估计值就等于它的先验估计，它就更加相信这个算出来的结果；当 K k = H − K_k=H^- Kk=H− 时最终的估计值就等于测量结果，它就更加相信这个测出来的结果
有了上面的分析我们的目标就变得非常清晰了，我们就是要去寻找一个合适的卡尔曼增益 K k K_k Kk 使得误差最小，也就是使得我们的估计值 X ^ k \hat{X}_k X^k 趋近于它的实际值 X k X_k Xk
大家现在可以去想一下，如何去选这个 K k K_k Kk 呢？
它的选择其实是和噪声相关的，因为比如说你测量的噪声特别大，那么你当然就会去愿意相信你计算出来的结果；而相反如果计算出来的这个误差特别大的话，你当然愿意去相信测量出来的结果，所以 K k K_k Kk 的取值和误差息息相关
如何去量化这个误差？这个误差是什么呢？
这时候我们引入误差 e k = X k − X ^ k e_k = X_k - \hat{X}_k ek=Xk−X^k，它也是符合正态分布，其概率分布如下：
P ( e ) ∼ ( 0 , P ) P(e) \sim (0,P) P(e)∼(0,P)
P P P 是它的协方差矩阵，具体计算还是和之前的一样
P = E [ e e T ] = [ V a r ( e 1 ) C o v ( e 1 , e 2 ) C o v ( e 2 , e 1 ) V a r ( e 2 ) ] P=E[ee^T]=\begin{bmatrix}\mathrm{Var}(e_1)&\mathrm{Cov}(e_1,e_2)\\\mathrm{Cov}(e_2,e_1)&\mathrm{Var}(e_2)\end{bmatrix} P=E[eeT]=[Var(e1)Cov(e2,e1)Cov(e1,e2)Var(e2)]
如果说我们新估计的这个结果 X ^ k \hat{X}_k X^k 距离实际值越小，那就说明它整个误差的方差最小，在之前有提到过，方差最小就说明它越接近于期望值越接近于 0。所以这个时候就很明确了，我们希望我们选取合适的 K k K_k Kk 使得 e k e_k ek 的协方差矩阵 P k P_k Pk 的 trace 最小，也就是迹最小
所以现在目标就是寻找合适的 K k K_k Kk 使得 t r ( P k ) tr(P_k) tr(Pk) 最小
下面我们就是要来计算它的这个协方差矩阵 P k P_k Pk 了
P k = E [ e k e k T ] = E [ ( X k − X ^ k ) ( X k − X ^ k ) T ] \begin{aligned} P_k &= E[e_ke_k^T] \\ &= E[(X_k - \hat{X}_k)(X_k - \hat{X}_k)^T] \end{aligned} Pk=E[ekekT]=E[(Xk−X^k)(Xk−X^k)T]
因为我们要求 K k K_k Kk 所有就把 X ^ k \hat{X}_k X^k 带入计算有：
( X k − X ^ k ) = X k − ( X ^ k − + K k ( Z k − H X ^ k − ) ) = X k − X ^ k − − K k Z k + K k H X ^ k − = X k − X ^ k − − K k ( H X k + V k ) + K k H X ^ k − = X k − X ^ k − − K k H X k − K k V k + K k H X ^ k − = ( X k − X ^ k − ) − K k H ( X k − X ^ k − ) − K k V k = ( I − K k H ) ( X k − X ^ k − ) − K k V k \begin{aligned} (X_k - \hat{X}_k) &= X_k - (\hat{X}_k^- + K_k(Z_k-H\hat{X}_k^-)) \\ &= X_k - \hat{X}_k^- - K_kZ_k + K_kH\hat{X}_k^- \\ &= X_k - \hat{X}_k^- - K_k(HX_k+V_k) + K_kH\hat{X}_k^- \\ &= X_k - \hat{X}_k^- - K_kHX_k - K_kV_k + K_kH\hat{X}_k^- \\ &= (X_k - \hat{X}_k^-) - K_kH(X_k-\hat{X}_k^-) - K_kV_k \\ &= (I - K_kH)(X_k - \hat{X}_k^-) - K_kV_k \\ \end{aligned} (Xk−X^k)=Xk−(X^k−+Kk(Zk−HX^k−))=Xk−X^k−−KkZk+KkHX^k−=Xk−X^k−−Kk(HXk+Vk)+KkHX^k−=Xk−X^k−−KkHXk−KkVk+KkHX^k−=(Xk−X^k−)−KkH(Xk−X^k−)−KkVk=(I−KkH)(Xk−X^k−)−KkVk
由 e k = X k − X ^ k e_k = X_k - \hat{X}_k ek=Xk−X^k 我们可以定义 e k − = ( X k − X ^ k − ) e_k^- = (X_k - \hat{X}_k^-) ek−=(Xk−X^k−) 其中 e k − e_k^- ek− 表示 e k e_k ek 的先验
将上式带入协方差矩阵 P P P 的计算中有：
P k = E [ e k e k T ] = E [ ( X k − X ^ k ) ( X k − X ^ k ) T ] = E [ ( ( I − K k H ) e k − − K k V k ) ( ( I − K k H ) e k − − K k V k ) T ] \begin{aligned} P_k &= E[e_ke_k^T] \\ &= E[(X_k - \hat{X}_k)(X_k - \hat{X}_k)^T] \\ &= E[((I - K_kH)e_k^- - K_kV_k)((I - K_kH)e_k^- - K_kV_k)^T] \end{aligned} Pk=E[ekekT]=E[(Xk−X^k)(Xk−X^k)T]=E[((I−KkH)ek−−KkVk)((I−KkH)ek−−KkVk)T]
假设现在有 A A A B B B 两个矩阵，矩阵论知识告诉我们有： ( A B ) T = B T A T (AB)^T = B^TA^T (AB)T=BTAT， ( A + B ) T = A T + B T (A+B)^T = A^T + B^T (A+B)T=AT+BT
因此有 ( ( I − K k H ) e k − − K k V k ) T = e k − T ( I − K k H ) T − V k T K k T ((I - K_kH)e_k^- - K_kV_k)^T = e_k^{-T}(I - K_kH)^T-V_k^TK_k^T ((I−KkH)ek−−KkVk)T=ek−T(I−KkH)T−VkTKkT
代入后展开则有：
P k = E [ e k e k T ] = E [ ( X k − X ^ k ) ( X k − X ^ k ) T ] = E [ ( ( I − K k H ) e k − − K k V k ) ( ( I − K k H ) e k − − K k V k ) T ] = E [ ( ( I − K k H ) e k − − K k V k ) ( e k − T ( I − K k H ) T − V k T K k T ) ] = E [ ( I − K k H ) e k − e k − T ( I − K k H ) T − ( I − K k H ) e k − V k T K k T − K k V k e k − T ( I − K k H ) T + K k V k V k T K k T ] \begin{aligned} P_k &= E[e_ke_k^T] \\ &= E[(X_k - \hat{X}_k)(X_k - \hat{X}_k)^T] \\ &= E[((I - K_kH)e_k^- - K_kV_k)((I - K_kH)e_k^- - K_kV_k)^T] \\ &= E[((I - K_kH)e_k^- - K_kV_k)(e_k^{-T}(I - K_kH)^T-V_k^TK_k^T)] \\ &= E[(I - K_kH)e_k^-e_k^{-T}(I - K_kH)^T - (I - K_kH)e_k^-V_k^TK_k^T - K_kV_ke_k^{-T}(I - K_kH)^T + K_kV_kV_k^TK_k^T] \end{aligned} Pk=E[ekekT]=E[(Xk−X^k)(Xk−X^k)T]=E[((I−KkH)ek−−KkVk)((I−KkH)ek−−KkVk)T]=E[((I−KkH)ek−−KkVk)(ek−T(I−KkH)T−VkTKkT)]=E[(I−KkH)ek−ek−T(I−KkH)T−(I−KkH)ek−VkTKkT−KkVkek−T(I−KkH)T+KkVkVkTKkT]
对上式求期望就是对每一项分别求期望，我们先来分析中间两项即 E [ ( I − K k H ) e k − V k T K k T ] E[(I - K_kH)e_k^-V_k^TK_k^T] E[(I−KkH)ek−VkTKkT] 以及 E [ K k V k e k − T ( I − K k H ) T ] E[K_kV_ke_k^{-T}(I - K_kH)^T] E[KkVkek−T(I−KkH)T]，由于 I I I、 K k K_k Kk、 H H H 都是常数可以提取出去，因此原来的两项就等价于 ( I − K k H ) E [ e k − V k T ] K k T (I - K_kH)E[e_k^-V_k^T]K_k^T (I−KkH)E[ek−VkT]KkT 和 K k E [ V k e k − T ] ( I − K k H ) T K_kE[V_ke_k^{-T}](I - K_kH)^T KkE[Vkek−T](I−KkH)T
其中 e k − e_k^- ek− 是先验误差 V k V_k Vk 是测量误差，二者没有关系，即二者相互独立
又已知如果两个随机变量 A A A B B B 相互独立，那么它们乘积的期望就等于它们各自期望的乘积，即 E ( A B ) = E ( A ) E ( B ) E(AB) = E(A)E(B) E(AB)=E(A)E(B)
有 E [ e k − V k T ] = E [ e k − ] E [ V k T ] E[e_k^-V_k^T] = E[e_k^-]E[V_k^T] E[ek−VkT]=E[ek−]E[VkT]， E [ V k e k − T ] = E [ V k ] E [ e k − T ] E[V_ke_k^{-T}] = E[V_k]E[e_k^{-T}] E[Vkek−T]=E[Vk]E[ek−T]
之前提到过 e k − e_k^- ek− 和 V k V_k Vk 都是服从均值为 0 的正态分布，它的期望为 0
因此有 E [ e k − V k T ] = 0 E[e_k^-V_k^T]=0 E[ek−VkT]=0 E [ V k e k − T ] = 0 E[V_ke_k^{-T}]=0 E[Vkek−T]=0，所以协方差矩阵 P P P 的中间两项就被干掉了
代入后有：
P k = E [ e k e k T ] = E [ ( X k − X ^ k ) ( X k − X ^ k ) T ] = E [ ( ( I − K k H ) e k − − K k V k ) ( ( I − K k H ) e k − − K k V k ) T ] = E [ ( ( I − K k H ) e k − − K k V k ) ( e k − T ( I − K k H ) T − V k T K k T ) ] = E [ ( I − K k H ) e k − e k − T ( I − K k H ) T − ( I − K k H ) e k − V k T K k T − K k V k e k − T ( I − K k H ) T + K k V k V k T K k T ] = E [ ( I − K k H ) e k − e k − T ( I − K k H ) T ] − E [ ( I − K k H ) e k − V k T K k T ] − E [ K k V k e k − T ( I − K k H ) T ] + E [ K k V k V k T K k T ] = E [ ( I − K k H ) e k − e k − T ( I − K k H ) T ] + E [ K k V k V k T K k T ] = ( I − K k H ) E [ e k − e k − T ] ( I − K k H ) T + K k E [ V k V k T ] K k T \begin{aligned} P_k &= E[e_ke_k^T] \\ &= E[(X_k - \hat{X}_k)(X_k - \hat{X}_k)^T] \\ &= E[((I - K_kH)e_k^- - K_kV_k)((I - K_kH)e_k^- - K_kV_k)^T] \\ &= E[((I - K_kH)e_k^- - K_kV_k)(e_k^{-T}(I - K_kH)^T-V_k^TK_k^T)] \\ &= E[(I - K_kH)e_k^-e_k^{-T}(I - K_kH)^T - (I - K_kH)e_k^-V_k^TK_k^T - K_kV_ke_k^{-T}(I - K_kH)^T + K_kV_kV_k^TK_k^T] \\ &= E[(I - K_kH)e_k^-e_k^{-T}(I - K_kH)^T] - E[(I - K_kH)e_k^-V_k^TK_k^T] - E[K_kV_ke_k^{-T}(I - K_kH)^T] + E[K_kV_kV_k^TK_k^T] \\ &= E[(I - K_kH)e_k^-e_k^{-T}(I - K_kH)^T] + E[K_kV_kV_k^TK_k^T] \\ &= (I - K_kH)E[e_k^-e_k^{-T}](I - K_kH)^T+K_kE[V_kV_k^T]K_k^T \end{aligned} Pk=E[ekekT]=E[(Xk−X^k)(Xk−X^k)T]=E[((I−KkH)ek−−KkVk)((I−KkH)ek−−KkVk)T]=E[((I−KkH)ek−−KkVk)(ek−T(I−KkH)T−VkTKkT)]=E[(I−KkH)ek−ek−T(I−KkH)T−(I−KkH)ek−VkTKkT−KkVkek−T(I−KkH)T+KkVkVkTKkT]=E[(I−KkH)ek−ek−T(I−KkH)T]−E[(I−KkH)ek−VkTKkT]−E[KkVkek−T(I−KkH)T]+E[KkVkVkTKkT]=E[(I−KkH)ek−ek−T(I−KkH)T]+E[KkVkVkTKkT]=(I−KkH)E[ek−ek−T](I−KkH)T+KkE[VkVkT]KkT
由 P = E [ e e T ] P=E[ee^T] P=E[eeT] 我们可以令 P k − = E [ e k − e k − T ] P_k^- = E[e_k^-e_k^{-T}] Pk−=E[ek−ek−T]；由 P ( V ) ∼ ( 0 , R ) P(V) \sim (0,R) P(V)∼(0,R) 可知 E [ V V T ] = R E[VV^T]=R E[VVT]=R
代入并展开有：
P k = E [ e k e k T ] = E [ ( X k − X ^ k ) ( X k − X ^ k ) T ] = E [ ( ( I − K k H ) e k − − K k V k ) ( ( I − K k H ) e k − − K k V k ) T ] = E [ ( ( I − K k H ) e k − − K k V k ) ( e k − T ( I − K k H ) T − V k T K k T ) ] = E [ ( I − K k H ) e k − e k − T ( I − K k H ) T − ( I − K k H ) e k − V k T K k T − K k V k e k − T ( I − K k H ) T + K k V k V k T K k T ] = E [ ( I − K k H ) e k − e k − T ( I − K k H ) T ] − E [ ( I − K k H ) e k − V k T K k T ] − E [ K k V k e k − T ( I − K k H ) T ] + E [ K k V k V k T K k T ] = E [ ( I − K k H ) e k − e k − T ( I − K k H ) T ] + E [ K k V k V k T K k T ] = ( I − K k H ) E [ e k − e k − T ] ( I − K k H ) T + K k E [ V k V k T ] K k T = ( I − K k H ) P k − ( I − K k H ) T + K k R K k T = ( P k − − K k H P k − ) ( I − 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 K k T + K k R K k T \begin{aligned} P_k &= E[e_ke_k^T] \\ &= E[(X_k - \hat{X}_k)(X_k - \hat{X}_k)^T] \\ &= E[((I - K_kH)e_k^- - K_kV_k)((I - K_kH)e_k^- - K_kV_k)^T] \\ &= E[((I - K_kH)e_k^- - K_kV_k)(e_k^{-T}(I - K_kH)^T-V_k^TK_k^T)] \\ &= E[(I - K_kH)e_k^-e_k^{-T}(I - K_kH)^T - (I - K_kH)e_k^-V_k^TK_k^T - K_kV_ke_k^{-T}(I - K_kH)^T + K_kV_kV_k^TK_k^T] \\ &= E[(I - K_kH)e_k^-e_k^{-T}(I - K_kH)^T] - E[(I - K_kH)e_k^-V_k^TK_k^T] - E[K_kV_ke_k^{-T}(I - K_kH)^T] + E[K_kV_kV_k^TK_k^T] \\ &= E[(I - K_kH)e_k^-e_k^{-T}(I - K_kH)^T] + E[K_kV_kV_k^TK_k^T] \\ &= (I - K_kH)E[e_k^-e_k^{-T}](I - K_kH)^T+K_kE[V_kV_k^T]K_k^T \\ &= (I - K_kH)P_k^-(I - K_kH)^T+K_kRK_k^T \\ &= (P_k^--K_kHP_k^-)(I-H^TK_k^T)+K_kRK_k^T \\ &= P_k^--K_kHP_k^--P_k^-H^TK_k^T+K_kHP_k^-H^TK_k^T+K_kRK_k^T \\ \end{aligned} Pk=E[ekekT]=E[(Xk−X^k)(Xk−X^k)T]=E[((I−KkH)ek−−KkVk)((I−KkH)ek−−KkVk)T]=E[((I−KkH)ek−−KkVk)(ek−T(I−KkH)T−VkTKkT)]=E[(I−KkH)ek−ek−T(I−KkH)T−(I−KkH)ek−VkTKkT−KkVkek−T(I−KkH)T+KkVkVkTKkT]=E[(I−KkH)ek−ek−T(I−KkH)T]−E[(I−KkH)ek−VkTKkT]−E[KkVkek−T(I−KkH)T]+E[KkVkVkTKkT]=E[(I−KkH)ek−ek−T(I−KkH)T]+E[KkVkVkTKkT]=(I−KkH)E[ek−ek−T](I−KkH)T+KkE[VkVkT]KkT=(I−KkH)Pk−(I−KkH)T+KkRKkT=(Pk−−KkHPk−)(I−HTKkT)+KkRKkT=Pk−−KkHPk−−Pk−HTKkT+KkHPk−HTKkT+KkRKkT
回过头去看下我们的目标是使得 t r ( P k ) tr(P_k) tr(Pk) 最小，这个时候我们就要去求取 P k P_k Pk 的迹了
我们先来看下 ( K k H P k − ) T = P k − T H T K k T = P k − H T K k T (K_kHP_k^-)^T= P_k^{-T}H^TK_k^T = P_k^{-}H^TK_k^T (KkHPk−)T=Pk−THTKkT=Pk−HTKkT，（协方差矩阵是对称矩阵，因此它的转置就是它本身）这两项互为转置，则它们的迹相同。因为迹是求取的对角线上的元素之和，对于任意矩阵 A A A 都会有 t r ( A ) = t r ( A T ) tr(A)=tr(A^T) tr(A)=tr(AT)
因此 t r ( P k ) tr(P_k) tr(Pk) 的计算就如下式所示：
t r ( P k ) = t r ( P k − ) − 2 t r ( K k H P k − ) + t r ( K k H P k − H T K k T ) + t r ( K k R K k T ) \begin{aligned} tr(P_k) &= tr(P_k^-) - 2tr(K_kHP_k^-) + tr(K_kHP_k^-H^TK_k^T) + tr(K_kRK_k^T) \\ \end{aligned} tr(Pk)=tr(Pk−)−2tr(KkHPk−)+tr(KkHPk−HTKkT)+tr(KkRKkT)
我们要寻找合适的 K k K_k Kk 使得 t r ( P k ) tr(P_k) tr(Pk) 有最小值，我们直接让它对 K k K_k Kk 求导求取极值点就可以了，即令 d t r ( P k ) d K k = 0 \frac{dtr(P_k)}{dK_k}=0 dKkdtr(Pk)=0
在正式开始计算之前，我们先给出两个结论方便我们后续使用：
d t r ( A B ) d A = B T d t r ( A B A T ) d A = A B + A B T \begin{aligned} \frac{dtr(AB)}{dA} &= B^T \\ \frac{dtr(ABA^T)}{dA} &= AB + AB^T \end{aligned} dAdtr(AB)dAdtr(ABAT)=BT=AB+ABT
我们来看看这两个表达式具体是如何推导的呢？
我们先来推导 d t r ( A B ) d A = B T \frac{dtr(AB)}{dA} = B^T dAdtr(AB)=BT
首先我们需要记住迹的定义和一些性质：
tr ( A ) = tr ( A T ) tr ( A B ) = tr ( B A ) \begin{aligned} \text{tr}(A) &= \text{tr}(A^T) \\ \text{tr}(AB) &= \text{tr}(BA) \end{aligned} tr(A)tr(AB)=tr(AT)=tr(BA)
我们将目标函数定义为 f ( A ) = tr ( A B ) f(A) = \text{tr}(AB) f(A)=tr(AB)，展开迹的定义，我们得到：
f ( A ) = tr ( A B ) = ∑ i = 1 n ( A B ) i i = ∑ i = 1 n ∑ k = 1 n a i k b k i f(A) = \text{tr}(AB) = \sum_{i=1}^n (AB)_{ii} = \sum_{i=1}^n \sum_{k=1}^n a_{ik}b_{ki} f(A)=tr(AB)=i=1∑n(AB)ii=i=1∑nk=1∑naikbki
现在我们想要找到函数 f ( A ) f(A) f(A) 关于一个特定元素 a i j a_{ij} aij 的偏导数，我们可以写出：
∂ f ( A ) ∂ a i j = ∑ k = 1 n b k j \frac{\partial f(A)}{\partial a_{ij}} = \sum_{k=1}^n b_{kj} ∂aij∂f(A)=k=1∑nbkj
由于 i i i 和 j j j 是任意的，我们可以得出结论：
d tr ( A B ) d A = B T \frac{d\text{tr}(AB)}{dA} = B^T dAdtr(AB)=BT
我们再来推导 d t r ( A B A T ) d A = A B + A B T \frac{dtr(ABA^T)}{dA} = AB+AB^T dAdtr(ABAT)=AB+ABT
我们的目标是找到 f ( A ) = d d A tr ( A B A T ) f(A) = \frac{d}{dA}\text{tr}(ABA^T) f(A)=dAdtr(ABAT)，首先我们可以将这个表达式写为：
f ( A ) = tr ( A B A T ) f(A) = \text{tr}(ABA^T) f(A)=tr(ABAT)
我们可以将其分解为两部分考虑，我们令 X = A B X=AB X=AB，首先我们找到函数 f ( A ) f(A) f(A) 关于 X X X 的导数，然后找打 X X X 关于 A A A 的导数。
首先我们有：
f ( A ) = tr ( X A T ) f(A) = \text{tr}(XA^T) f(A)=tr(XAT)
我们可以找打 f ( A ) f(A) f(A) 关于 X X X 的导数：
d f d X = A \frac{df}{dX} = A dXdf=A
然后再找到 X X X 关于 A A A 的导数：
d X d A = B T \frac{dX}{dA} = B^T dAdX=BT
我们使用链式法则来找到 f ( A ) f(A) f(A) 关于 A A A 的导数：
d f d A = d f d X d X d A = A B T \frac{df}{dA} = \frac{df}{dX}\frac{dX}{dA} = AB^T dAdf=dXdfdAdX=ABT
但是我们还需要找到 X A T XA^T XAT 部分关于 A A A 的导数，为了找到这个导数，我们可以使用以下性质：
d d A tr ( X A T ) = d d A tr ( A T X ) \frac{d}{dA}\text{tr}(XA^T) = \frac{d}{dA}\text{tr}(A^TX) dAdtr(XAT)=dAdtr(ATX)
然后我们可以利用迹的性质来找到这个导数：
d d A tr ( A T X ) = X = A B \frac{d}{dA}\text{tr}(A^TX) = X = AB dAdtr(ATX)=X=AB
现在我们可以将这两部分加在一起来找到整体的导数：
d d A tr ( A B A T ) = A B + A B T \frac{d}{dA}\text{tr}(ABA^T) = AB + AB^T dAdtr(ABAT)=AB+ABT
注：上述推导都并不非常严谨，主要是为了方便理解
有了上述结论，我们再来计算 t r ( P k ) d K k \frac{tr(P_k)}{dK_k} dKktr(Pk)
t r ( P k ) d K k = t r ( P k − ) d K k − 2 t r ( K k H P k − ) d K k + t r ( K k H P k − H T K k T ) d K k + t r ( K k R K k T ) d K k = 0 − 2 ( H P k − ) T + ( K k H P k − H T + K k ( H P k − H T ) T ) + ( K k R + K k R T ) = 0 − 2 ( H P k − ) T + ( K k H P k − H T + K k H P k − T H T ) + ( K k R + K k R T ) = 0 − 2 ( H P k − ) T + ( K k H P k − H T + K k H P k − H T ) + ( K k R + K k R ) = 0 − 2 ( H P k − ) T + 2 K k H P k − H T + 2 K k R \begin{aligned} \frac{tr(P_k)}{dK_k} &= \frac{tr(P_k^-)}{dK_k} - 2\frac{tr(K_kHP_k^-)}{dK_k} + \frac{tr(K_kHP_k^-H^TK_k^T)}{dK_k} + \frac{tr(K_kRK_k^T)}{dK_k} \\ &= 0 - 2(HP_k^-)^T + (K_kHP_k^-H^T+K_k(HP_k^-H^T)^T) + (K_kR+K_kR^T) \\ &= 0 - 2(HP_k^-)^T + (K_kHP_k^-H^T+K_kHP_k^{-T}H^T) + (K_kR + K_kR^T)\\ &= 0 - 2(HP_k^-)^T + (K_kHP_k^-H^T+K_kHP_k^{-}H^T) + (K_kR + K_kR)\\ &= 0 - 2(HP_k^-)^T + 2K_kHP_k^-H^T + 2K_kR \end{aligned} dKktr(Pk)=dKktr(Pk−)−2dKktr(KkHPk−)+dKktr(KkHPk−HTKkT)+dKktr(KkRKkT)=0−2(HPk−)T+(KkHPk−HT+Kk(HPk−HT)T)+(KkR+KkRT)=0−2(HPk−)T+(KkHPk−HT+KkHPk−THT)+(KkR+KkRT)=0−2(HPk−)T+(KkHPk−HT+KkHPk−HT)+(KkR+KkR)=0−2(HPk−)T+2KkHPk−HT+2KkR
值得注意的是由于先验误差协方差矩阵 P k − P_k^- Pk− 和测量误差协方差矩阵 R R R 都是对称矩阵，因此有 P k − = P k − T P_k^- = P_k^{-T} Pk−=Pk−T 以及 R = R T R = R^T R=RT
令 d t r ( P k ) d K k = 0 \frac{dtr(P_k)}{dK_k}=0 dKkdtr(Pk)=0 则有：
− 2 ( H P k − ) T + 2 K k H P k − H T + 2 K k R = 0 − P k − H T + K k ( H P k − H T + R ) = 0 K k ( H P k − H T + R ) = P k − H T K k = P k − H T H P k − H T + R \begin{aligned}- 2(HP_k^-)^T + 2K_kHP_k^-H^T + 2K_kR &= 0 \\ -P_k^-H^T+K_k(HP_k^-H^T+R) &= 0 \\ K_k(HP_k^-H^T+R) &= P_k^-H^T \\ \color{red}K_k = \frac{P_k^-H^T}{HP_k^-H^T+R} \end{aligned} −2(HPk−)T+2KkHPk−HT+2KkR−Pk−HT+Kk(HPk−HT+R)Kk(HPk−HT+R)Kk=HPk−HT+RPk−HT=0=0=Pk−HT
上述这个公式就是卡尔曼滤波器中最核心的公式，把 Kalman Gain 计算了出来，
其实去分析它也可以看出，当噪声协方差矩阵 R R R 特别大时， K k → 0 K_k \to 0 Kk→0 X ^ k = X ^ k − \hat{X}_k = \hat{X}_k^- X^k=X^k−，估计值等于先验估计，因为它的测量噪声非常大，测量噪声非常大的时候也可以得出结论，我们愿意去相信这个先验的结果，也就是愿意去相信计算出来的结果，
如果 R R R 很小的话，也就是 R → 0 R \to 0 R→0 也就是我们的测量误差非常非常小，此时 K k = H − K_k = H^- Kk=H−
前面有提到过， K k = H − K_k = H^- Kk=H− 的时候， X ^ k = H − Z k \hat{X}_k = H^-Z_k X^k=H−Zk 也就是说估计值等于测量出来的结果。非常的好理解，当我们的测量误差 R R R 很小的时候，它就是测量结果，当测量误差 R R R 很大的时候，它就是我们估计的结果
到此为止，卡尔曼增益的整个推导就完成了
下节内容我们会去推导剩下的两个公式，再配合上本节的三个公式，就是卡尔曼滤波的五大公式了。
结语
本篇博客详细推导了卡尔曼增益，这个还是需要各位看官自己多动手理解的。下篇文章我们将会继续推导误差协方差矩阵，敬请期待
参考
- 常用的MarkDown颜色 笔记
- 【卡尔曼滤波器】_Kalman_Filter_全网最详细数学推导【博主强烈推荐】