Kalman Filter: Recursive Data Processing

• The Kalman filter is a recursive data processing algorithm.
• It optimally estimates the state of a dynamic system.
• It uses a series of incomplete and noisy measurements.
• Applications include navigation, robotics, and economics.

System Model

• Kalman filters assume the true state at time $k$ evolves from the state at $(k-1)$.
• This is represented by the equation: $\mathbf{x}_k = \mathbf{F}k \mathbf{x}{k-1} + \mathbf{B}_k \mathbf{u}_k + \mathbf{w}_k$
• A measurement $\mathbf{z}_k$ is made that is $\mathbf{z}_k = \mathbf{H}_k \mathbf{x}_k + \mathbf{v}_k$
• $\mathbf{F}_k$ is the state transition model.
• $\mathbf{B}_k$ is the control-input model.
• $\mathbf{u}_k$ is the control vector.
• $\mathbf{H}_k$ is the observation model.
• $\mathbf{w}_k$ and $\mathbf{v}_k$ represent process and measurement noise, respectively.
• Noise is assumed to be zero-mean Gaussian with covariance $\mathbf{Q}_k$ and $\mathbf{R}_k$:
  • $p(\mathbf{w}) \sim N(0, \mathbf{Q}_k)$
  • $p(\mathbf{v}) \sim N(0, \mathbf{R}_k)$

Algorithm Steps

• The Kalman filter algorithm is completed in two steps, predict and update

Predict

• Project the state ahead: $\hat{\mathbf{x}}_k^- = \mathbf{F}k \hat{\mathbf{x}}{k-1} + \mathbf{B}_k \mathbf{u}_k$
• Project the error covariance ahead: $\mathbf{P}_k^- = \mathbf{F}k \mathbf{P}{k-1} \mathbf{F}_k^T + \mathbf{Q}_k$

Update

• Compute the Kalman Gain: $\mathbf{K}_k = \mathbf{P}_k^- \mathbf{H}_k^T (\mathbf{H}_k \mathbf{P}_k^- \mathbf{H}_k^T + \mathbf{R}_k)^{-1}$
• Update estimate with measurement $\mathbf{z}_k$: $\hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- + \mathbf{K}_k (\mathbf{z}_k - \mathbf{H}_k \hat{\mathbf{x}}_k^-)$
• Update the error covariance: $\mathbf{P}_k = (I - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_k^-$