卡尔曼预测更新
predict:
x^i−=Ax^i−1+Bui−1\widehat{x}_i^{-} = A\widehat{x}_{i-1}+Bu_{i-1}xi−=Axi−1+Bui−1
correction:
x^i=x^i−+k(yi−Hx^i−)\widehat{x}_i = \widehat{x}_i^{-}+k(y_i-H\widehat{x}_i^{-})xi=xi−+k(yi−Hxi−)
其中,kkk为kalman gain matrix; yiy_iyi观测值; AAA状态转移矩阵; HHH测量矩阵; B控制矩阵。
计算预测值、预测值和真实值之间的误差协防差矩阵
x^k−=Ax^k−1−+Buk−1\widehat{x}_k^{-} = A\widehat{x}_{k-1}^{-}+Bu_{k-1}xk−=Axk−1−+Buk−1
Pk−=APk−1AT+QP_k^{-}=AP_{k-1}A^T+QPk−=APk−1AT+Q
QQQ系统误差
计算卡尔曼增益kkk,再得到估计值
Kk=Pk−HT(HPK−HT+R)−1K_k=P_k^{-}H^T(HP_K^{-}H^T+R)^{-1}Kk=Pk−HT(HPK−HT+R)−1
x^k=Ax^k−+Kk(Zk−Hx^k−)\widehat{x}_k = A\widehat{x}_k^{-}+K_k(Z_k-H\widehat{x}_k^{-})xk=Axk−+Kk(Zk−Hxk−)
RRR测量误差
计算估计值和真实值之间的误差协防差矩阵
Pk=(1−KkH)Pk−P_k=(1-K_kH)P_k^{-}Pk=(1−KkH)Pk−
卡尔曼转换关系
估计值==》预测值==》估计值==》…\ldots…估计值==》预测值==》估计值==》…\ldots…
核心在于计算卡尔慢增益kkk