This post details the Kalman filter equations.
Predict
State prediction:
Where:
-
is the predicted state at time step
.
-
is the estimate of state at time step
.
-
is the transition matrix. It describes how the state will change according to the previous state (prediction).
-
is a matrix that translates control input at time step
into a predicted change in state. In another words, it maps an input vector
into the state space.
-
is the system input at time step
.
Uncertainty (or covariance) prediction:
Where:
-
is the error covariance matrix predicted at time step
.
-
is the estimated error covariance matrix associated with the estimated state
.
-
is the system noise covariance matrix.
Update
Innovation or measurement residual:
Where:
-
is a measurement error : this is the difference between the measurement
and the estimate measurement from state
.
-
is an observation (or measurement) from the true state
.
-
is a transition matrix which maps the state space into the observed space.
Innovation (or residual) covariance:
Where:
-
is the covariance matrix associated to the measurement error
.
-
is the covariance matrix for the measurement noise.
Optimal Kalman gain
Where
-
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:
-
is the identity matrix.