卡尔曼滤波器5大公式 HQ的码拉松 2021-12-22 225 阅读1分钟 计算先验估计[X1,k−^X2,k−^]\left[ \begin{matrix} \hat{X_{1,k}^-} \\ \hat{X_{2,k}^-}\end{matrix} \right][X1,k−^X2,k−^] = A [X1,k−1^X2,k−1^]\left[ \begin{matrix} \hat{X_{1,k-1}} \\ \hat{X_{2,k-1}}\end{matrix} \right][X1,k−1^X2,k−1^] 计算先验的误差协方差Pk−=APk−1AT+QP_k^- = AP_{k-1}A^T + QPk−=APk−1AT+Q 计算卡尔曼增益Kk=Pk−HT(HPk−1−HT+R)K_k = \frac {P_k^-H^T} {(HP_{k-1}^-H^T + R)}Kk=(HPk−1−HT+R)Pk−HT 后验估计Xk=Xk−^+Kk(Zk−HXk−^)X_k = \hat{X_{k}^-} + K_k(Z_k - H\hat{X_k^-} )Xk=Xk−^+Kk(Zk−HXk−^) 更新误差协方差Pk=(I−KkH)Pk−P_k = (I - K_kH)P_k^-Pk=(I−KkH)Pk−
