This post details the Kalman filter equations.
- 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:
- 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.
Innovation or measurement residual:
- 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:
- is the covariance matrix associated to the measurement error .
- is the covariance matrix for the measurement noise.
Optimal Kalman gain
- 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
- is the identity matrix.