卡尔曼滤波公式推导
Posted herr_edoc
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了卡尔曼滤波公式推导相关的知识，希望对你有一定的参考价值。
上一篇文章从概率密度函数的角度推导了卡尔曼滤波公式（卡尔曼滤波公式推导（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^-1z_k x^measure=Hk−1zk，通过加权平均（互补滤波）得到最终的后验估计值 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^-1z_k-\\hat x_k^-) x^k=x^k−+G(Hk−1zk−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−Hkx^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(ekekT)，最小二乘法是一种最优线性无偏估计算法，此处将问题转化为求矩阵 P k P_k Pk的迹 t r ( P k ) tr(P_k) tr(Pk)最小。
公式推导过程中需要用到一些知识点辅助，下面我们先来了解几个公式（每个公式标注了序号，后面推导会用到）。
- 一个 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\\mathrmdtr(AB)\\mathrmdA=B^T_(2) dAdtr(AB)=B(2)T， d t r ( A B A T ) d A = 2 A B ( 3 ) \\frac\\mathrmdtr(ABA^T)\\mathrmdA=2AB_(3) dAdtr(ABAT)=2AB(3)
- 两个相互独立的变量相乘的期望等于它们的期望相乘，即 E ( A B ) = E ( A ) E ( B ) ( 4 ) E(AB)=E(A)E(B)_(4) E(AB)=E(A)E(B)(4)
- 协方差矩阵为对称矩阵，即有 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−卡尔曼滤波公式推导