Kalman filter

This post details the Kalman filter equations.

Predict

State prediction: $\hat{x}_{i}^p = F_i.\hat{x}_{i-1} + B_i.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_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$

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.

Update

Innovation or measurement residual: $\widetilde{y}_i = z_i - H_i.\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_i$ is a transition matrix 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.

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.