卡尔曼滤波总结

\[先验:\hat{x}_{k}^{-}=A \hat{x}_{k+1}+B u_{k+1} \\ 先验协方差: P_{k}^{-}=A P_{k-1} A^{\top}+Q \]

\[后验:\hat{x}_{k}=\hat{x}_{k}^{-}+K_{k}\left(z_{k}-H \hat{x}_{k}^{-}\right) \\ 后验协方差: P_{k}=\left(I-K_{k} H\right) P_{k}^{-} \]