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.
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)
1.2 calculate a new process covariance (how certain is the model?)
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:
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:
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:
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