predict： x ^ i − = A x ^ i − 1 + B u i − 1 \widehat{x}_i^{-} = A\widehat{x}_{i-1}+Bu_{i-1} x i−=Ax i−1+Bui−1
correction: x ^ i = x ^ i − + k ( y i − H x ^ i − ) \widehat{x}_i = \widehat{x}_i^{-}+k(y_i-H\widehat{x}_i^{-}) x i=x i−+k(yi−Hx i−)
其中， k k k为kalman gain matrix; y i y_i yi观测值; A A A状态转移矩阵; H H H测量矩阵; B控制矩阵。
x ^ k − = A x ^ k − 1 − + B u k − 1 \widehat{x}_k^{-} = A\widehat{x}_{k-1}^{-}+Bu_{k-1} x k−=Ax k−1−+Buk−1
P k − = A P k − 1 A T + Q P_k^{-}=AP_{k-1}A^T+Q Pk−=APk−1AT+Q
Q Q Q系统误差
K k = P k − H T ( H P K − H T + R ) − 1 K_k=P_k^{-}H^T(HP_K^{-}H^T+R)^{-1} Kk=Pk−HT(HPK−HT+R)−1
x ^ k = A x ^ k − + K k ( Z k − H x ^ k − ) \widehat{x}_k = A\widehat{x}_k^{-}+K_k(Z_k-H\widehat{x}_k^{-}) x k=Ax k−+Kk(Zk−Hx k−) R R R测量误差
P k = ( 1 − K k H ) P k − P_k=(1-K_kH)P_k^{-} Pk=(1−KkH)Pk−
估计值==》预测值==》估计值==》 … \ldots …估计值==》预测值==》估计值==》 … \ldots … 核心在于计算卡尔慢增益 k k k
