Extended Kalman filter (EKF)

This post details the Kalman filter equations.

Predict

State prediction:

 \hat{x}_{i}^p = f(\hat{x}_{i-1},u_i)

Where:


  • \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 is differential function that describes how the state will change according to the previous state (prediction) and the system input (u_i).
  • 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

Where:


  • 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.
  • F_i is the transition matrix. It is given by the Jacobian of f()=\frac{df}{dx}

Update

Innovation or measurement residual:

 \widetilde{y}_i = z_i - h(\hat{x}_i^p)

Where:


  • \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 is a differential function which maps the state space into the observed space.

Innovation (or residual) covariance:

 S_i=H_i.P_i^p.H_i^\top+R_i

Where:


  • S_i is the covariance matrix associated to the measurement error \widetilde{y}_i.
  • R_i is the covariance matrix for the measurement noise.
  • H_i is a transition matrix which maps the state space into the observed space. It is given by H_i=\frac{dh}{dx}

Near-optimal Kalman gain:

 K_i=P_i^p.H_i^\top.S_i^{-1}

Where


  • 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

 P_i=(I-K_i.H_i).P_i^p

Where:


  • I is the identity matrix.

Leave a Reply

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