Kalman filter

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_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:

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.

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_i$ is a transition matrix 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.

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: