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

xn+1=Gnxn+Bun+wnyn=Hnxn+vn\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 kk states x\vec x, transition matrix G\ma G, gaussian process noise wn\vec w_n, input u\vec u, ll measurements y\vec y, measurement model H\ma H, gaussian measurement noise vn\vec v_n, time point nn.

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

If there are no known inputs to the process, then Bun=0\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 Pn\ma P_n. This matrix is influenced by the process noise wn\vec w_n and the measurement noise vn\vec v_n. During prediction, the process noise increases the uncertainty, whereas combining it with the measurement during the update phase decreases the uncertainty.

wn∼N(0,Qn)vn∼N(0,Rn)\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 Q\ma Q are larger than values of R\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)

x^n∣n−1=Gnx^n−1∣n−1\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?)

Pxn∣n−1=GnPxn−1∣n−1Gn⊤+Qn\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: Δyn=yn−y^n∣n−1=yn−Hnx^n∣n−1\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: S=HnPxn∣n−1Hn⊤+Rn\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:

Kn=Pxn∣n−1Hn⊤HnPxn∣n−1Hn⊤+Rn=Pxn∣n−1Hn⊤S−1\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 ll measurements:

x^n∣n=x^n∣n−1+KnΔyn\(\hat{\vec x}_{n|n} = \hat{\vec x}_{n|n-1} + \ma K_n \Delta \vec y_n\) which is the estimation of xn\vec x_n based on Δyn\Delta y_n with Kij∈[0.0;1.0]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:

Pxn∣n=Pxn∣n−1+KnHnPxn∣n−1\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.

xn=g(xn−1,un)+wnyn=h(xn−1)+vn\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()g() and h()h() are non-linear functions. For covariance the Jacobi-Matrix of the model is used:

G=∂g∂x∣x^n−1,unH=∂h∂x∣x^n\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 ii sensors measure the same state xjx_j, this can be expressed in the measurement matrix H\ma H. The column jj will have ii rows with entries.

Example for Gyroscope and Accelerometer

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

References#