control//state estimation//Kalman filter//predict-update cycle
The loop a Kalman filter runs, one tick of the system clock at a time. Predict: apply the model to the last estimate and let the uncertainty grow. Measure: when a reading arrives, compare it with the prediction. Correct: move the estimate by a fraction of the disagreement and shrink the uncertainty. In the scalar case it is six lines:
The loop a Kalman filter runs, one tick of the system clock at a time. Predict: apply the model to the last estimate and let the uncertainty grow. Measure: when a reading arrives, compare it with the prediction. Correct: move the estimate by a fraction of the disagreement and shrink the uncertainty. In the scalar case it is six lines:
x^k−=Fx^k−1,Pk−=FPk−1F+Q,yk=zk−x^k−,Kk=Pk−Pk−+R,x^k=x^k−+Kkyk,Pk=(1−Kk)Pk−.\begin{gathered} \hat x_k^-=F\hat x_{k-1},\qquad P_k^-=FP_{k-1}F+Q,\\[4pt] y_k=z_k-\hat x_k^-,\qquad K_k=\frac{P_k^-}{P_k^-+R},\\[4pt] \hat x_k=\hat x_k^-+K_ky_k,\qquad P_k=(1-K_k)P_k^-. \end{gathered}x^k−=Fx^k−1,Pk−=FPk−1F+Q,yk=zk−x^k−,Kk=Pk−+RPk−,x^k=x^k−+Kkyk,Pk=(1−Kk)Pk−.
Predict the state, predict its uncertainty, compare, judge, correct, update the uncertainty. Memorize the words before the symbols: predict, measure, compare, correct according to confidence, repeat.
The loop is not always complete. Prediction runs every tick; correction only when a measurement arrives. A drone predicts with its IMU hundreds of times per second and hears the GPS five to ten times. In a tunnel it only predicts (dead reckoning), and PPP grows with every blind step, telling honestly how lost it is getting.
With vectors the same lines hold with matrices, P−=FPFT+QP^-=FPF^{\mathsf T}+QP−=FPFT+Q and a gain that translates through HHH (state-space model, covariance propagation).
?If the estimate feeds the next prediction, doesn't the model get contaminated?
No, and that is fine: the new estimate is not the prediction. What always stays separate is the model, the predicting mathematics FFF and QQQ, and physics does not change because a GPS said something. What is updated is the state (x^,P)(\hat x,P)(x^,P), the best belief of the moment, and that is exactly what the next prediction should start from (a priori and a posteriori).