卡尔曼滤波公式推导（2）

上一篇文章从概率密度函数的角度推导了卡尔曼滤波公式（卡尔曼滤波公式推导（1）），接下来从矩阵的最小二乘法的角度来推导。

在预测和更新阶段分别能得到两个近似状态向量真实值的值，记为 x ^ p r e d i c t = x ^ k − \hat x^{predict}=\hat x_k^{-} x^predict=x^k−​和 x ^ m e a s u r e = H k − 1 z k \hat x^{measure}=H_k^{-1}z_k x^measure=Hk−1​zk​，通过加权平均（互补滤波）得到最终的后验估计值 x ^ k = x ^ p r e d i c t + G ( x ^ m e a s u r e − x ^ p r e d i c t ) \hat x_k=\hat x^{predict}+G(\hat x^{measure}-\hat x^{predict}) x^k​=x^predict+G(x^measure−x^predict)，即 x ^ k = x ^ k − + G ( H k − 1 z k − x ^ k − ) \hat x_k=\hat x_k^{-}+G(H_k^{-1}z_k-\hat x_k^{-}) x^k​=x^k−​+G(Hk−1​zk​−x^k−​)，为了避免矩阵求逆，令 G = K H k G=KH_k G=KHk​，则 x ^ k = x ^ k − + K ( z k − H k x ^ k − ) \hat x_k=\hat x_k^{-}+K(z_k-H_k\hat x_k^{-}) x^k​=x^k−​+K(zk​−Hk​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 ∼ N ( 0 , P k ) e_k\sim N(0, P_k) ek​∼N(0,Pk​)， P k = E ( e k e k T ) P_k=E(e_ke_k^{T}) Pk​=E(ek​ekT​)，最小二乘法是一种最优线性无偏估计算法，此处将问题转化为求矩阵 P k P_k Pk​的迹 t r ( P k ) tr(P_k) tr(Pk​)最小。

公式推导过程中需要用到一些知识点辅助，下面我们先来了解几个公式（每个公式标注了序号，后面推导会用到）。

1. 一个 n ∗ n n*n n∗n矩阵 A n A_n An​主对角线上所有元素的和称为矩阵的迹，记为 t r ( A n ) tr(A_n) tr(An​)，并且有 t r ( A T ) = t r ( A ) ( 1 ) tr(A^{T})=tr(A)_{(1)} tr(AT)=tr(A)(1)​， d t r ( A B ) d A = B ( 2 ) T \frac{\mathrm{d}tr(AB)}{\mathrm{d}A}=B^{T}_{(2)} dAdtr(AB)​=B(2)T​， d t r ( A B A T ) d A = 2 A B ( 3 ) \frac{\mathrm{d}tr(ABA^{T})}{\mathrm{d}A}=2AB_{(3)} dAdtr(ABAT)​=2AB(3)​
2. 两个相互独立的变量相乘的期望等于它们的期望相乘，即 E ( A B ) = E ( A ) E ( B ) ( 4 ) E(AB)=E(A)E(B)_{(4)} E(AB)=E(A)E(B)(4)​
3. 协方差矩阵为对称矩阵，即有 P = P ( 5 ) T P=P^{T}_{(5)} P=P(5)T​，且其主对角线元素为各个变量的方差。

下面继续推导，将变量代入 e k = x k − x ^ k e_k=x_k-\hat x_k ek​=xk​−x^k​，有 e k = x k − ( x ^ k − + K ( z k − H k x ^ k − ) e_k=x_k-(\hat x_k^{-}+K(z_k-H_k\hat x_k^{-}) ek​=xk​−(x^k−​+K(zk​−Hk​x^k−​)，因为 z k = H k x k − v k z_k=H_kx_k-v_k zk​=Hk​xk​−vk​，所以 e k = x k − ( x ^ k − + K ( H k x k − v k − H k x ^ k − ) ) e_k=x_k-(\hat x_k^{-}+K(H_kx_k-v_k-H_k\hat x_k^{-})) ek​=xk​−(x^k−​+K(Hk​xk​−vk​−Hk​x^k−​))，展开并合并同类项得 e k = ( I − K H k ) ( x k − x ^ k − ) + K v k e_k=(I-KH_k)(x_k-\hat x_k^{-})+Kv_k ek​=(I−KHk​)(xk​−x^k−​)+Kvk​，又因为先验估计值和真实值误差 e ^ k − = x k − x ^ k − \hat e_k^{-}=x_k-\hat x_k^{-} e^k−​=xk​−x^k−​，所以， e k = ( I − K H k ) e ^ k − + K v k e_k=(I-KH_k)\hat e_k^{-}+Kv_k ek​=(I−KHk​)e^k−​+Kvk​，则:
e k e k T = ( ( I − K H k ) e ^ k − + K v k ) ( ( I − K H k ) e ^ k − + K v k ) T = ( ( I − K H k ) e ^ k − + K v k ) ( e ^ k − T ( I − H k T K T ) + v k T K T ) = ( I − K H k ) e ^ k − e ^ k − T ( I − H k T K T ) + K v k e ^ k − T ( I − H k T K T ) + ( I − K H k ) e ^ k − K v k + + K v k v k T K T \begin{aligned} e_ke_k^{T}= & ((I-KH_k)\hat e_k^{-}+Kv_k)((I-KH_k)\hat e_k^{-}+Kv_k)^{T} \\ =& ((I-KH_k)\hat e_k^{-}+Kv_k)(\hat e_k^{-T}(I-H_k^{T}K^{T})+v_k^{T}K^{T}) \\ = & (I-KH_k)\hat e_k^{-}\hat e_k^{-T}(I-H_k^{T}K^{T}) \\ & +Kv_k\hat e_k^{-T}(I-H_k^{T}K^{T})\\ & +(I-KH_k)\hat e_k^{-}Kv_k+ \\ & +Kv_kv_k^{T}K^{T} \end{aligned} ek​ekT​===​((I−KHk​)e^k−​+Kvk​)((I−KHk​)e^k−​+Kvk​)T((I−KHk​)e^k−​+Kvk​)(e^k−T​(I−HkT​KT)+vkT​KT)(I−KHk​)e^k−​e^k−T​(I−HkT​KT)+Kvk​e^k−T​(I−HkT​KT)+(I−KHk​)e^k−​Kvk​++Kvk​vkT​KT​
则：
P k = E ( e k e k T ) = ( I − K H k ) E ( e ^ k − e ^ k − T ) ( I − H k T K T ) + K E ( v k ) E ( e ^ k − T ) ( I − H k T K T ) 根据公式(4) + ( I − K H k ) E ( e ^ k − ) K E ( v k ) 根据公式(4) + K E ( v k v k T ) K T \begin{aligned} P_k= & E(e_ke_k^{T}) \\ = & (I-KH_k)E(\hat e_k^{-}\hat e_k^{-T})(I-H_k^{T}K^{T}) \\ & +KE(v_k)E(\hat e_k^{-T})(I-H_k^{T}K^{T}) &\text{根据公式(4)} \\ & +(I-KH_k)E(\hat e_k^{-})KE(v_k) &\text{根据公式(4)}\\ & +KE(v_kv_k^{T})K^{T} \end{aligned} Pk​==​E(ek​ekT​)(I−KHk​)E(e^k−​e^k−T​)(I−HkT​KT)+KE(vk​)E(e^k−T​)(I−HkT​KT)+(I−KHk​)E(e^k−​)KE(vk​)+KE(vk​vkT​)KT​根据公式(4)根据公式(4)​
基于假设 v k ∼ N ( 0 , R k ) v_k\sim N(0, R_k) vk​∼N(0,Rk​)， e ^ k − ∼ N ( 0 , P k − ) \hat e_k^{-}\sim N(0, P_k^{-}) e^k−​∼N(0,Pk−​)，有 E ( v k ) = 0 E(v_k)=0 E(vk​)=0 ， E ( e ^ k − ) = 0 E(\hat e_k^{-})=0 E(e^k−​)=0， E ( v k v k T ) = R k E(v_kv_k^{T})=R_k E(vk​vkT​)=Rk​ ， E ( e ^ k − e ^ k − T ) = P k − E(\hat e_k^{-}\hat e_k^{-T})=P_k^{-} E(e^k−​e^k−T​)=Pk−​，所以：
P k = ( I − K H k ) P k − ( I − H k T K T ) + K R k K T = P k − − K H k P k − − P k − H k T K T + K H k P k − H k T K T + K R k K T \begin{aligned} P_k= & (I-KH_k)P_k^{-}(I-H_k^{T}K^{T}) \\ & +KR_kK^{T} \\ = & P_k^{-}-KH_kP_k^{-}-P_k^{-}H_k^{T}K^{T}+KH_kP_k^{-}H_k^{T}K^{T}+KR_kK^{T} \end{aligned} Pk​==​(I−KHk​)Pk−​(I−HkT​KT)+KRk​KTPk−​−KHk​Pk−​−Pk−​HkT​KT+KHk​Pk−​HkT​KT+KRk​KT​
为了让 P k P_k Pk​的迹 t r ( P k ) tr(P_k) tr(Pk​)最小，可以令 t r ( P k ) tr(P_k) tr(Pk​)对 K K K求导的结果为0，即：
d t r ( P k ) d K = d P k − d K (a) − d K H k P k − d K (b) − d P k − H k T K T d K (c) + d K H k P k − H k T K T d K (d) + d K R k K T d K (e) = 0 \begin{aligned} \frac{\mathrm{d}tr(P_k)}{\mathrm{d}K}=& \frac{\mathrm{d}P_k^{-}}{\mathrm{d}K} & \text{(a)}\\ & -\frac{\mathrm{d}KH_kP_k^{-}}{\mathrm{d}K} & \text{(b)}\\ & -\frac{\mathrm{d}P_k^{-}H_k^{T}K^{T}}{\mathrm{d}K} & \text{(c)}\\ & +\frac{\mathrm{d}KH_kP_k^{-}H_k^{T}K^{T}}{\mathrm{d}K} & \text{(d)} \\ & +\frac{\mathrm{d}KR_kK^{T}}{\mathrm{d}K} & \text{(e)} \\ & =0 \end{aligned} dKdtr(Pk​)​=​dKdPk−​​−dKdKHk​Pk−​​−dKdPk−​HkT​KT​+dKdKHk​Pk−​HkT​KT​+dKdKRk​KT​=0​(a)(b)(c)(d)(e)​
其中a项为0；b项根据公式(2)有 − d K H k P k − d K = − P k − T H k T -\frac{\mathrm{d}KH_kP_k^{-}}{\mathrm{d}K}=-P_k^{-T}H_k^{T} −dKdKHk​Pk−​​=−Pk−T​HkT​，又因为公式(5)有 − P k − T H k T = − P k − H k T -P_k^{-T}H_k^{T}=-P_k^{-}H_k^{T} −Pk−T​HkT​=−Pk−​HkT​；c项根据公式(1)有 − d P k − H k T K T d K = − d K H k P k − T d K = − P k − H k T -\frac{\mathrm{d}P_k^{-}H_k^{T}K^{T}}{\mathrm{d}K}=-\frac{\mathrm{d}KH_kP_k^{-T}}{\mathrm{d}K}=-P_k^{-}H_k^{T} −dKdPk−​HkT​KT​=−dKdKHk​Pk−T​​=−Pk−​HkT​；d项、e项根据公式(3)分别有 d K H k P k − H k T K T d K = − 2 K H k P k − H k T \frac{\mathrm{d}KH_kP_k^{-}H_k^{T}K^{T}}{\mathrm{d}K}=-2KH_kP_k^{-}H_k^{T} dKdKHk​Pk−​HkT​KT​=−2KHk​Pk−​HkT​、 d K R k K T d K = 2 K R k \frac{\mathrm{d}KR_kK^{T}}{\mathrm{d}K}=2KR_k dKdKRk​KT​=2KRk​，所以：
d t r ( P k ) d K = − 2 P k − H k T + 2 K H k P k − H k T + 2 K R k = 0 \frac{\mathrm{d}tr(P_k)}{\mathrm{d}K}=-2P_k^{-}H_k^{T}+2KH_kP_k^{-}H_k^{T}+2KR_k=0 dKdtr(Pk​)​=−2Pk−​HkT​+2KHk​Pk−​HkT​+2KRk​=0
经过简单的代数运算可以得到：
K = P k − H k T ( H k P k − H k T + R k ) − 1 K=P_k^{-}H_k^{T}(H_kP_k^{-}H_k^{T}+R_k)^{-1} K=Pk−​HkT​(Hk​Pk−​HkT​+Rk​)−1
到这里卡尔曼增益K已经推导出来了，可以将 K K K代入上面 P k P_k Pk​的等式最后两项，得：
P k = P k − − K H k P k − − P k − H k T K T + K ( H k P k − H k T K T + R k ) K T = P k − − K H k P k − − P k − H k T K T + P k − H k T ( H k P k − H k T + R k ) − 1 ( H k P k − H k T + R k ) K T = P k − − K H k P k − − P k − H k T K T + P k − H k T K T = P k − − K H k P k − \begin{aligned} P_k= & P_k^{-}-KH_kP_k^{-}-P_k^{-}H_k^{T}K^{T}+K(H_kP_k^{-}H_k^{T}K^{T}+R_k)K^{T} \\ = & P_k^{-}-KH_kP_k^{-}-P_k^{-}H_k^{T}K^{T}+P_k^{-}H_k^{T}(H_kP_k^{-}H_k^{T}+R_k)^{-1}(H_kP_k^{-}H_k^{T}+R_k)K^{T} \\ = & P_k^{-}-KH_kP_k^{-}-P_k^{-}H_k^{T}K^{T}+P_k^{-}H_k^{T}K^{T} \\ = & P_k^{-}-KH_kP_k^{-} \end{aligned} Pk​====​Pk−​−KHk​Pk−​−Pk−​HkT​KT+K(Hk​Pk−​HkT​KT+Rk​)KTPk−​−KHk​Pk−​−Pk−​HkT​KT+Pk−​HkT​(Hk​Pk−​HkT​+Rk​)−1(Hk​Pk−​HkT​+Rk​)KTPk−​−KHk​Pk−​−Pk−​HkT​KT+Pk−​HkT​KTPk−​−KHk​Pk−​​
而 P k − = E ( e ^ k − e ^ k − T ) = E ( ( x k − x ^ k − ) ( x k − x ^ k − ) T ) P_k^{-}=E(\hat e_k^{-}\hat e_k^{-T})=E((x_k-\hat x_k^{-})(x_k-\hat x_k^{-})^{T}) Pk−​=E(e^k−​e^k−T​)=E((xk​−x^k−​)(xk​−x^k−​)T)，可以将 x k = A k x k − 1 + B k u k + w k x_k=A_kx_{k-1}+B_ku_{k}+w_k xk​=Ak​xk−1​+Bk​uk​+wk​、 x ^ k − = A k x ^ k − 1 + B k u k \hat x_k^{-}=A_k\hat x_{k-1}+B_ku_k x^k−​=Ak​x^k−1​+Bk​uk​代入得到：
P k − = E ( ( A k x k − 1 + B k u k + w k − A k x ^ k − 1 − B k u k ) ( A k x k − 1 + B k u k + w k − A k x ^ k − 1 − B k u k ) T ) = E ( ( A k ( x k − 1 − x ^ k − 1 ) + w k ) ( A k ( x k − 1 − x ^ k − 1 ) + w k ) T ) = E ( ( A k e ^ k − 1 + w k ) ( A k e ^ k − 1 + w k ) T ) = E ( ( A k e ^ k − 1 + w k ) ( e ^ k − 1 T A k T + w k T ) ) = E ( A k e ^ k − 1 e ^ k − 1 T A k T + w k e ^ k − 1 T A k T + A k e ^ k − 1 w k T + w k w k T ) = E ( A k e ^ k − 1 e ^ k − 1 T A k T ) + E ( w k e ^ k − 1 T A k T ) + E ( A k e ^ k − 1 w k T ) + E ( w k w k T ) = A k P k − 1 A k T + 0 + 0 + Q k = A k P k − 1 A k T + Q k \begin{aligned} P_k^{-}= & E((A_kx_{k-1}+B_ku_{k}+w_k-A_k\hat x_{k-1}-B_ku_k)(A_kx_{k-1}+B_ku_{k}+w_k-A_k\hat x_{k-1}-B_ku_k)^{T}) \\ = & E((A_k(x_{k-1}-\hat x_{k-1})+w_k)(A_k(x_{k-1}-\hat x_{k-1})+w_k)^{T}) \\ = & E((A_k\hat e_{k-1}+w_k)(A_k\hat e_{k-1}+w_k)^{T}) \\ = & E((A_k\hat e_{k-1}+w_k)(\hat e_{k-1}^{T}A_k^{T}+w_k^{T})) \\ = & E(A_k\hat e_{k-1}\hat e_{k-1}^{T}A_k^{T}+w_k\hat e_{k-1}^{T}A_k^{T}+A_k\hat e_{k-1}w_k^{T}+w_kw_k^{T}) \\ = & E(A_k\hat e_{k-1}\hat e_{k-1}^{T}A_k^{T})+E(w_k\hat e_{k-1}^{T}A_k^{T})+E(A_k\hat e_{k-1}w_k^{T})+E(w_kw_k^{T}) \\ = & A_kP_{k-1}A_k^{T}+0+0+Q_k \\ = & A_kP_{k-1}A_k^{T}+Q_k \end{aligned} Pk−​========​E((Ak​xk−1​+Bk​uk​+wk​−Ak​x^k−1​−Bk​uk​)(Ak​xk−1​+Bk​uk​+wk​−Ak​x^k−1​−Bk​uk​)T)E((Ak​(xk−1​−x^k−1​)+wk​)(Ak​(xk−1​−x^k−1​)+wk​)T)E((Ak​e^k−1​+wk​)(Ak​e^k−1​+wk​)T)E((Ak​e^k−1​+wk​)(e^k−1T​AkT​+wkT​))E(Ak​e^k−1​e^k−1T​AkT​+wk​e^k−1T​AkT​+Ak​e^k−1​wkT​+wk​wkT​)E(Ak​e^k−1​e^k−1T​AkT​)+E(wk​e^k−1T​AkT​)+E(Ak​e^k−1​wkT​)+E(wk​wkT​)Ak​Pk−1​AkT​+0+0+Qk​Ak​Pk−1​AkT​+Qk​​
P k − P_k^{-} Pk−​的推导基于假设 w k ∼ N ( 0 , Q k ) w_k\sim N(0, Q_k) wk​∼N(0,Qk​)、 e ^ k ∼ N ( 0 , P k ) \hat e_k\sim N(0, P_k) e^k​∼N(0,Pk​)。
以上推导完卡尔曼滤波的所有公式。在推导的过程中我们也发现了应用卡尔曼滤波的前提条件：
（1）系统应该是线性的
（2）过程噪声和测量噪声是相互独立的，且都必须满足正态分布
如果满足以上条件，则卡尔曼滤波是一个线性最优无偏估计算法。

在实际的工程应用中，在误差允许范围内，有的系统模型可以通过一些近似运算来构造成线性系统，但是有的系统非线性程度比较严重，直接应用卡尔曼滤波会出现较大的误差，对于这种系统，可以使用EKF（扩展卡尔曼滤波器）、UKF（无迹卡尔曼滤波器）或者PF（粒子滤波），后续文章会继续介绍这几种滤波器。

标签：推导,公式,卡尔曼滤波,KH,HkT,KT,Pk,hat,mathrm
来源： https://blog.csdn.net/trailbrazer/article/details/119283400