深圳深圳网站制作长沙手机网站建设公司哪家好

当前位置： 首页 > news >正文

深圳深圳网站制作,长沙手机网站建设公司哪家好,wordpress前台显示异常后台进不去,WordPress怎么开启rest目录 前言数学理论推导卡尔曼增益超详细数学推导结语参考 前言 最近项目需求涉及到目标跟踪部分#xff0c;准备从 DeepSORT 多目标跟踪算法入手。DeepSORT 中涉及的内容有点多#xff0c;以前也就对其进行了简单的了解#xff0c;但是真正去做发现总是存在这样或者那样的困… 目录 前言数学理论推导卡尔曼增益超详细数学推导结语参考 前言 最近项目需求涉及到目标跟踪部分准备从 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} Xk AX{k-1}BU{k}W{k-1} \ Z_k HXkV{k} \end{aligned} Xk​Zk​​AXk−1​BUk​Wk−1​HXk​Vk​​ 这里面 X k Xk 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] QE[WWT]其中 E E E 代表了期望那这个公式怎么来的呢我们用一个简单的例子来说明一下。 假设我们现在有两项即 X [ x 1 x 2 ] X \begin{bmatrix}x_1\x_2\end{bmatrix} X[x1​x2​​]那它的过程噪声自然而然就是 w 1 w_1 w1​ w 2 w2 w2即 W [ w 1 w 2 ] W \begin{bmatrix}w_1\w_2\end{bmatrix} W[w1​w2​​] 我们可以将 Q E [ W W T ] QE[WW^T] QE[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 ] ] QE\left[\begin{bmatrix}w_1\w_2\end{bmatrix}\begin{bmatrix}w_1w_2\end{bmatrix}\right]E\left[\begin{bmatrix}w_1^2w_1w_2\w_2w_1w_2^2\end{bmatrix}\right] QE[[w1​w2​​][w1​​w2​​]]E[[w12​w2​w1​​w1​w2​w22​​]] 现在我们可以通过单独考虑每个元素来计算期望值。所以我们得到 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[w2​w1​]​E[w1​w2​]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[w1​w2​]−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[w1​w2​] 这样我们可以将协方差写入矩阵的非对角元素中。对于对角元素我们有方差 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]^2E[w_1^2] Var(w1​)E[w12​]−E[w1​]2E[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 Zk Zk​ 都是现实当中的一种情况但是我们在建模的过程当中 W k − 1 W{k-1} Wk−1​ 和 V k Vk 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 Xk 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 Zk 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 G0 G0 时 X ^ k X ^ k − \hat{X}_k \hat{X}_k^- X^k​X^k−​当 G 1 G1 G1 时 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 GKk​H 代入上式有 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_k0 Kk​0 时 X ^ k X ^ k − \hat{X}_k \hat{X}_k^- X^k​X^k−​当 K k H − K_kH^- Kk​H− 时 X ^ k H − Z k \hat{X}_k H^-Z_k X^k​H−Zk​ 即 K k 0 K_k0 Kk​0 时最终的估计值就等于它的先验估计它就更加相信这个算出来的结果当 K k H − K_kH^- 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 ) ] PE[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} PE[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[ek​ekT​]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_kV_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−​−Kk​Zk​Kk​HX^k−​Xk​−X^k−​−Kk​(HXk​Vk​)Kk​HX^k−​Xk​−X^k−​−Kk​HXk​−Kk​Vk​Kk​HX^k−​(Xk​−X^k−​)−Kk​H(Xk​−X^k−​)−Kk​Vk​(I−Kk​H)(Xk​−X^k−​)−Kk​Vk​​ 由 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[ek​ekT​]E[(Xk​−X^k​)(Xk​−X^k​)T]E[((I−Kk​H)ek−​−Kk​Vk​)((I−Kk​H)ek−​−Kk​Vk​)T]​ 假设现在有 A A A B B B 两个矩阵矩阵论知识告诉我们有 ( A B ) T B T A T (AB)^T B^TA^T (AB)TBTAT ( A B ) T A T B T (AB)^T A^T B^T (AB)TATBT 因此有 ( ( 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−Kk​H)ek−​−Kk​Vk​)Tek−T​(I−Kk​H)T−VkT​KkT​ 代入后展开则有 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[ek​ekT​]E[(Xk​−X^k​)(Xk​−X^k​)T]E[((I−Kk​H)ek−​−Kk​Vk​)((I−Kk​H)ek−​−Kk​Vk​)T]E[((I−Kk​H)ek−​−Kk​Vk​)(ek−T​(I−Kk​H)T−VkT​KkT​)]E[(I−Kk​H)ek−​ek−T​(I−Kk​H)T−(I−Kk​H)ek−​VkT​KkT​−Kk​Vk​ek−T​(I−Kk​H)TKk​Vk​VkT​KkT​]​ 对上式求期望就是对每一项分别求期望我们先来分析中间两项即 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−Kk​H)ek−​VkT​KkT​] 以及 E [ K k V k e k − T ( I − K k H ) T ] E[K_kV_ke_k^{-T}(I - K_kH)^T] E[Kk​Vk​ek−T​(I−Kk​H)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−Kk​H)E[ek−​VkT​]KkT​ 和 K k E V k e k − T T K_kEV_ke_k^{-T}^T Kk​EVk​ek−T​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[Vk​ek−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[Vk​ek−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 E e k − e k − T 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)Ee_k^-e_k^{-T}^TK_kE[V_kV_k^T]K_k^T \end{aligned} Pk​​E[ek​ekT​]E[(Xk​−X^k​)(Xk​−X^k​)T]E[((I−Kk​H)ek−​−Kk​Vk​)((I−Kk​H)ek−​−Kk​Vk​)T]E[((I−Kk​H)ek−​−Kk​Vk​)(ek−T​(I−Kk​H)T−VkT​KkT​)]E[(I−Kk​H)ek−​ek−T​(I−Kk​H)T−(I−Kk​H)ek−​VkT​KkT​−Kk​Vk​ek−T​(I−Kk​H)TKk​Vk​VkT​KkT​]E[(I−Kk​H)ek−​ek−T​(I−Kk​H)T]−E[(I−Kk​H)ek−​VkT​KkT​]−E[Kk​Vk​ek−T​(I−Kk​H)T]E[Kk​Vk​VkT​KkT​]E[(I−Kk​H)ek−​ek−T​(I−Kk​H)T]EKk​Vk​VkT​KkT​Eek−​ek−T​TKk​E[Vk​VkT​]KkT​​ 由 P E [ e e T ] PE[ee^T] PE[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 E e k − e k − T 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)Ee_k^-e_k^{-T}^TK_kE[V_kV_k^T]K_k^T \ (I - K_kH)P_k^-(I - K_kH)^TK_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^TK_kHP_k^-H^TK_k^TK_kRK_k^T \ \end{aligned} Pk​​E[ek​ekT​]E[(Xk​−X^k​)(Xk​−X^k​)T]E[((I−Kk​H)ek−​−Kk​Vk​)((I−Kk​H)ek−​−Kk​Vk​)T]E[((I−Kk​H)ek−​−Kk​Vk​)(ek−T​(I−Kk​H)T−VkT​KkT​)]E[(I−Kk​H)ek−​ek−T​(I−Kk​H)T−(I−Kk​H)ek−​VkT​KkT​−Kk​Vk​ek−T​(I−Kk​H)TKk​Vk​VkT​KkT​]E[(I−Kk​H)ek−​ek−T​(I−Kk​H)T]−E[(I−Kk​H)ek−​VkT​KkT​]−E[Kk​Vk​ek−T​(I−Kk​H)T]E[Kk​Vk​VkT​KkT​]E[(I−Kk​H)ek−​ek−T​(I−Kk​H)T]EKk​Vk​VkT​KkT​Eek−​ek−T​TKk​E[Vk​VkT​]KkT​(I−Kk​H)Pk−​(I−Kk​H)TKk​RKkT​(Pk−​−Kk​HPk−​)(I−HTKkT​)Kk​RKkT​Pk−​−Kk​HPk−​−Pk−​HTKkT​Kk​HPk−​HTKkT​Kk​RKkT​​ 回过头去看下我们的目标是使得 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 (Kk​HPk−​)TPk−T​HTKkT​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(Kk​HPk−​)tr(Kk​HPk−​HTKkT​)tr(Kk​RKkT​)​ 我们要寻找合适的 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)}{dKk}0 dKk​dtr(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)​​BTABABT​ 我们来看看这两个表达式具体是如何推导的呢 我们先来推导 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{i1}^n (AB){ii} \sum{i1}^n \sum{k1}^n a{ik}b{ki} f(A)tr(AB)i1∑n​(AB)ii​i1∑n​k1∑n​aik​bki​ 现在我们想要找到函数 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{k1}^n b_{kj} ∂aij​∂f(A)​k1∑n​bkj​ 由于 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} ABAB^T dAdtr(ABAT)​ABABT 我们的目标是找到 f ( A ) d d A tr ( A B A T ) f(A) \frac{d}{dA}\text{tr}(ABA^T) f(A)dAd​tr(ABAT)首先我们可以将这个表达式写为 f ( A ) tr ( A B A T ) f(A) \text{tr}(ABA^T) f(A)tr(ABAT) 我们可以将其分解为两部分考虑我们令 X A B XAB XAB首先我们找到函数 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​dXdf​dAdX​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) dAd​tr(XAT)dAd​tr(ATX) 然后我们可以利用迹的性质来找到这个导数 d d A tr ( A T X ) X A B \frac{d}{dA}\text{tr}(A^TX) X AB dAd​tr(ATX)XAB 现在我们可以将这两部分加在一起来找到整体的导数 d d A tr ( A B A T ) A B A B T \frac{d}{dA}\text{tr}(ABA^T) AB AB^T dAd​tr(ABAT)ABABT 注上述推导都并不非常严谨主要是为了方便理解 有了上述结论我们再来计算 t r ( P k ) d K k \frac{tr(P_k)}{dK_k} dKk​tr(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^TK_k(HP_k^-H^T)^T) (K_kRK_kR^T) \ 0 - 2(HP_k^-)^T (K_kHP_k^-H^TK_kHP_k^{-T}H^T) (K_kR K_kR^T)\ 0 - 2(HP_k^-)^T (K_kHP_k^-H^TK_kHP_k^{-}H^T) (K_kR K_kR)\ 0 - 2(HP_k^-)^T 2K_kHP_k^-H^T 2K_kR \end{aligned} dKk​tr(Pk​)​​dKk​tr(Pk−​)​−2dKk​tr(Kk​HPk−​)​dKk​tr(Kk​HPk−​HTKkT​)​dKk​tr(Kk​RKkT​)​0−2(HPk−​)T(Kk​HPk−​HTKk​(HPk−​HT)T)(Kk​RKk​RT)0−2(HPk−​)T(Kk​HPk−​HTKk​HPk−T​HT)(Kk​RKk​RT)0−2(HPk−​)T(Kk​HPk−​HTKk​HPk−​HT)(Kk​RKk​R)0−2(HPk−​)T2Kk​HPk−​HT2Kk​R​ 值得注意的是由于先验误差协方差矩阵 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 RRT 令 d t r ( P k ) d K k 0 \frac{dtr(P_k)}{dK_k}0 dKk​dtr(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^TK_k(HP_k^-H^TR) 0 \ K_k(HP_k^-H^TR) P_k^-H^T \ \color{red}K_k \frac{P_k^-H^T}{HP_k^-H^TR} \end{aligned} −2(HPk−​)T2Kk​HPk−​HT2Kk​R−Pk−​HTKk​(HPk−​HTR)Kk​(HPk−​HTR)Kk​HPk−​HTRPk−​HT​​00Pk−​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_全网最详细数学推导【博主强烈推荐】