Extended Kalman filter (EKF)

This post details the Kalman filter equations.

Predict

State prediction:

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:

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}$

Innovation or measurement residual:

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:

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:

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:

Updated estimate covariance

Where: