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 differential function that describes how the state will change according to the previous state (prediction) and the system input (
).
-
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.
-
is the transition matrix. It is given by the Jacobian of
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 differential function 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.
-
is a transition matrix which maps the state space into the observed space. It is given by
Near-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.