文章目录
卡尔曼滤波器：协方差矩阵
继续上期博客的推导，已知条件如下： x k = A x k − 1 + B u k − 1 + w k − 1 w ∼ P ( 0 , Q ) z k = H x k + v k v ∼ P ( 0 , R ) x_k = Ax_{k-1} + Bu_{k-1} + w_{k-1} \qquad w \sim P(0,Q) \\ \ \\ z_k = Hx_k + v_k \qquad \qquad \qquad \qquad v \sim P(0,R) xk=Axk−1+Buk−1+wk−1w∼P(0,Q) zk=Hxk+vkv∼P(0,R)
在上期博客中已推导出公式分别为先验估计、后验估计、卡尔曼增益，公式如下： x ^ k − = A x ^ k − 1 + B u k − 1 x ^ k = x ^ k − + K k ( z k − H x ^ k − ) K k = P k − H T H P k − H T + R \hat{x}_k^- = A\hat{x}_{k-1} + Bu_{k-1} \\ \ \\ \hat{x}_k = \hat{x}_k^- + K_k(z_k - H\hat{x}_k^-) \\ \ \\ K_k = \frac{P_k^-H^T}{HP_k^-H^T+R} x^k−=Ax^k−1+Buk−1 x^k=x^k−+Kk(zk−Hx^k−) Kk=HPk−HT+RPk−HT
已知 P k − = E [ e k − e k − T ] , e k − = x k − x ^ k − P_k^- = E[e_k^-e_k^{-T}], e_k^- = x_k - \hat{x}_k^- Pk−=E[ek−ek−T],ek−=xk−x^k−，有：
e k − = x k − x ^ k − = A x k − 1 + B u k − 1 + w k − 1 − A x ^ k − 1 + B u k − 1 = A ( x k − 1 − x ^ k − 1 ) + w k − 1 = A e k − 1 + w k − 1 P k − = E [ e k − e k − T ] = E [ ( A e k − 1 + w k − 1 ) ( A e k −
最低0.47元/天 解锁文章
1542
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
