# 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.