本文详细介绍了卡尔曼滤波器的数学推导，旨在解决含有噪声的状态估计问题。通过分析过程噪声和测量噪声的协方差，卡尔曼滤波器提供了一种优化估计的方法。文章阐述了如何利用卡尔曼增益Kk最小化估计误差协方差，以达到最佳估计效果。
卡尔曼滤波器：数学推导
一、卡尔曼滤波器要解决的问题
由上期博客可知，含误差的质量弹簧阻尼二阶系统状态空间方程为： x k = A x k − 1 + B u k − 1 + w k − 1 z k = H x k + v k {\bm x_k} = A{\bm x_{k-1}}+Bu_{k-1}+{\bm w_{k-1}} \\ \ \\{\bm z_k} = H{\bm x_k} + {\bm v_k} xk=Axk−1+Buk−1+wk−1 zk=Hxk+vk 其中， w w w 为过程噪声， v v v 为测量噪声，且噪声服从正态分布，即 p ( w ) ∼ ( 0 , Q ) p(w)\sim(0, Q) p(w)∼(0,Q)其中， 0 0 0 为期望值， Q Q Q 为协方差矩阵。根据协方差定义 Q = E [ w w T ] Q = E\left[ \begin{array}{ccc} w & w^T \end{array} \right] Q=E[wwT]，可得:
Q = E [ [ w 1 w 2 ] [ w 1 w 2 ] ] = [ E w 1 2 E w 1 w 2 E w 2 w 1 E w 2 2 ] = [ σ w 1 2 σ w 1 σ w 2 σ w 2 σ w 1 σ w 2 2 ] Q = E \left[ \begin{array}{ccc} \left[ \begin{array}{ccc} w_1 \\ w_2 \end{array} \right] \end{array} \left[ \begin{array}{ccc} w_1 & w_2 \end{array} \right] \right] \\ \ \\ = \left[ \begin{array}{ccc} Ew_1^2 & Ew_1w_2 \\ \ \\ Ew_2w_1 & Ew_2^2 \end{array} \right] \quad \ \\ \ \\ = \left[ \begin{array}{ccc} \sigma_{w_1}^2 & \sigma_{w_1}\sigma_{w_2} \\ \\ \sigma_{w_2}\sigma_{w_1} & \sigma_{w_2}^2 \end{array} \right] \quad Q=E[[w1w2][w1w2]] =⎣⎡Ew12 Ew2w1Ew1w2Ew22⎦⎤ =⎣⎡σw12σw2σw1σw1σw2σw22⎦⎤同理， p ( v ) ∼ ( 0 , R ) p(v)\sim(0,R) p(v)∼(0,R)。
上述表达式中 w , v w,v w,v 的值是随机的，无法用数学模型准确建模，因此我们使用卡尔曼滤波器进行优化，以使得结果最大程度接近真实值。
不考虑噪声时，状态空间方程为：
x ^ k − = A x ^ k − 1 + B u k − 1 (1) \hat{\bm x}_k^- = A \hat{\bm x}_{k-1}+Bu_{k-1} \tag{1} x^k−=Ax^k−1+Buk−1(1)
事实上噪声是无法避免的，因此公式（1）求得的计算结果为未处理的估计值，我们叫做先验估计值，记作 x ^ − \hat{\bm x}^- x^−。
对于测量值 z k {\bm z_k} zk，不考虑噪声时：
z k = H x k (2) {\bm z_k} = H{\bm x_k} \tag{2} zk=Hxk(2)
根据公式（2）可求得：
x ^ k m e a = H − z k (3) {\hat{\bm x}_k}_{mea} = H^-{\bm z_k} \tag{3} x^kmea=H−zk(3)
因为公式（1）（2）均未考虑噪声，因此 x ^ k , x ^ k m e a \hat{\bm x}_k,{\hat{\bm x}_k}_{mea} x^k,x^kmea 都是不准确的。
前面博客已经提到：
x ^ k = x ^ k − 1 + G ( z k − x ^ k − 1 ) (4) \hat{x}_k = \hat{x}_{k-1} + G(z_k - \hat{x}_{k-1}) \tag{4} x^k=x^k−1+G(zk−x^k−1)(4)
在本例中 z k = x ^ k m e a , x ^ k − 1 = x ^ k − z_k = {\hat{x}_k}_{mea},\ \hat{x}_{k-1} = \hat{x}_k^- zk=x^kmea, x^k−1=x^k−，将式（3）代入式（4）得：
x ^ k = x ^ k − + G ( H − z k − x ^ k − ) (5) \hat{x}_k = \hat{x}_k^- + G(H^-z_k - \hat{x}_k^-) \tag{5} x^k=x^k−+G(H−zk−x^k−)(5)
在教科书上一般表示 G = K k H G = K_kH G=KkH，代入式（5）中得后验估计：
x ^ k = x ^ k − + K k ( z k − H x ^ k − ) (6) \hat{x}_k = \hat{x}_k^- + K_k(z_k - H\hat{x}_k^-) \tag{6} x^k=x^k−+Kk(zk−Hx^k−)(6)
其中，
最低0.47元/天 解锁文章
1593
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
