This post details the Kalman filter equations.
- 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:
- 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
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 differential function 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.
- is a transition matrix which maps the state space into the observed space. It is given by
Near-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.