Skip to content

Kalman Filter#

The Kalman Filter is a linear quadratic estimator used to estimate the state of a system by combining sensor measurements and a (physical) process model of the system. It works optimal if measurements and process variations have white gaussian noise.

Working Principle#

The Kalman Filter averages a prediction of a system's state with a new measurement using a weighted average. The purpose of the weights is that estimated values with smaller uncertainty are "trusted" more. The result of the weighted average is a new state estimate that lies between the predicted and measured state, and has a smaller estimated uncertainty than either alone.

The Kalman filter works in two steps: predict and update.

Defining the Model#

  • system's process model
  • control inputs to that process
  • multiple sequential measurements (e.g.¬†from sensors)

State Space#

The dynamic model for the physical process is

\[\begin{array}{ll} \vec{x}_{n+1} & = \ma G_n \vec{x}_{n} + \ma B \vec{u}_n + \vec{w}_n \\ \vec{y}_{n} & = \ma H_{n} \vec{x}_{n} + \vec{v}_{n} \end{array}\]

with the \(k\) states \(\vec x\), transition matrix \(\ma G\), gaussian process noise \(\vec w_n\), input \(\vec u\), \(l\) measurements \(\vec y\), measurement model \(\ma H\), gaussian measurement noise \(\vec v_n\), time point \(n\).

The measurement matrix \(\ma H\) defines how the observations correspond to the state. If the state variables can be directly observed then \(\ma H = \ma 1\).

If there are no known inputs to the process, then \(\ma B \vec{u}_n = 0\) and this term can be removed.

Noise#

The overall uncertainty of the estimation is expressed with the covariance matrix \(\ma P_n\). This matrix is influenced by the process noise \(\vec w_n\) and the measurement noise \(\vec v_n\). During prediction, the process noise increases the uncertainty, whereas combining it with the measurement during the update phase decreases the uncertainty.

\[\begin{array}{ll} \vec{w}_n & \sim \mathcal{N}(\ma 0, \ma Q_n) \\ \vec{v}_n & \sim \mathcal{N}(\ma 0, \ma R_n) \end{array}\]

If values of \(\ma Q\) are larger than values of \(\ma R\), the filter trusts more the process, less the measurements.

Calculations#

1. Step: Prediction#

1.1 calculate the new (a priori) state estimate based on the old state and the dynamic model (e.g. physical laws)

\[\hat {\vec x}_{n|n-1} = \ma G_n \hat{\vec x}_{n-1|n-1}\]

1.2 calculate a new process covariance (how certain is the model?)

\[\ma P_{\vec x_{n|n-1}} = \ma G_n \ma P_{\vec x_{n-1|n-1}} \ma G_n^\top + \ma Q_n\]

2. Step: Update#

2.1 calculate intermediate values (optional):

  • Innovation: \(\Delta \vec y_n = \vec y_n - \hat{\vec y}_{n|n-1} =\vec y_n - \ma H_{n} \hat{\vec x}_{n|n-1}\)
    which are the real measurements minus predicted measurements

  • Innovation Covariance: \(\ma S = \ma H_{n} \ma P_{\vec x_{n|n-1}} \ma H_{n}^\top + \ma R_n\)

2.2 calculate optimal Kalman-gain:

\[\ma K_n = \frac{\ma P_{\vec x_{n|n-1}} \ma H_{n}^\top}{\ma H_{n} \ma P_{\vec x_{n|n-1}} \ma H_{n}^\top + \ma R_n} = \ma P_{\vec x_{n|n-1}} \ma H_{n}^\top {\ma S}^{-1}\]

2.3 calculate the updated (a posteriori) state estimate using \(l\) measurements:

\(\(\hat{\vec x}_{n|n} = \hat{\vec x}_{n|n-1} + \ma K_n \Delta \vec y_n\)\) which is the estimation of \(\vec x_n\) based on \(\Delta y_n\) with \(K_{ij} \in [0.0; 1.0]\) where 0.0 means the filter fully trusts the prediction and 1.0 means the filter fully trusts the measurement.

2.4 update process covariance:

\[\ma P_{\vec x_{n|n}} = \ma P_{\vec x_{n|n-1}} + \ma K_n \ma H_{n} \ma P_{\vec x_{n|n-1}}\]

Extended Kalman Filter#

The Extended Kalman Filter (EKF) uses non-linear dynamic models.

\[\begin{array}{ll} \vec{x}_{n} & = g(\vec{x}_{n-1}, \vec{u}_n) + \vec{w}_n \\[0.5em] \vec{y}_{n} & = h(\vec{x}_{n-1}) + \vec{v}_{n} \end{array}\]

where \(g()\) and \(h()\) are non-linear functions. For covariance the Jacobi-Matrix of the model is used:

\[\ma G = \frac{\partial g}{\partial x} \Big\vert_{\hat{\vec x}_{n-1}, \vec u_n} \qquad \ma H = \frac{\partial h}{\partial x} \Big\vert_{\hat{\vec x}_{n}}\]

Sensor Fusion#

If \(i\) sensors measure the same state \(x_j\), this can be expressed in the measurement matrix \(\ma H\). The column \(j\) will have \(i\) rows with entries.

Example for Gyroscope and Accelerometer

Kalman Filter for Gyroscope and Accelerometer: state \(\vec x\) are the orientation angles roll and pitch and the bias angle

References#