卡尔曼滤波器:数学推导
一、卡尔曼滤波器要解决的问题
由上期博客可知,含误差的质量弹簧阻尼二阶系统状态空间方程为: 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)
其中,