Kalman filter

This post details the Kalman filter equations.


State prediction:

 \hat{x}_{i}^p = F_i.\hat{x}_{i-1} + B_i.u_i


  • \hat{x}_i^p is the predicted state at time step i.
  • \hat{x}_{i} is the estimate of state at time step i.
  • F_i is the transition matrix. It describes how the state will change according to the previous state (prediction).
  • B_i is a matrix that translates control input at time step i into a predicted change in state. In another words, it maps an input vector u_i into the state space.
  • u_i is the system input at time step i.

Uncertainty (or covariance) prediction:

 P_i^p = F_i.P_{i-1}.F_i^\top + Q_i


  • P_i^p is the error covariance matrix predicted at time step i.
  • P_i is the estimated error covariance matrix associated with the estimated state \hat{x}_{i}.
  • Q_i is the system noise covariance matrix.


Innovation or measurement residual:

 \widetilde{y}_i = z_i - H_i.\hat{x}_i^p


  • \widetilde{y}_i is a measurement error : this is the difference between the measurement z_i and the estimate measurement from state \hat{x}_i^p.
  • z_i is an observation (or measurement) from the true state x_i.
  • H_i is a transition matrix which maps the state space into the observed space.

Innovation (or residual) covariance:



  • S_i is the covariance matrix associated to the measurement error \widetilde{y}_i.
  • R_i is the covariance matrix for the measurement noise.

Optimal Kalman gain



  • K_i is the Kalman gain, this matrix contains the balance between prediction and observations. This matrix will weight the merging between predicted state and observations.

Updated state estimate:

 \hat{x}_i=\hat{x}_i^p + K_i.\widetilde{y}_i

Updated estimate covariance



  • I is the identity matrix.

Leave a Reply

Your email address will not be published. Required fields are marked *