标签：推导 公式 卡尔曼滤波 KH HkT KT Pk hat mathrm

上一篇文章从概率密度函数的角度推导了卡尔曼滤波公式（卡尔曼滤波公式推导（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

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。