Kalman filters

Library

A C++ library for using Kalman Filters, Extended Kalman Filters (EKF) and Unscented Kalman Filters (UKF) is available on the easykf google code project. On the project page, there is also a document where the different filters are described. Below are basic independent python example usage of these filters.

Linear fitlers

Kalman filters were introduced by Kalman in the 1960 to solve the problem of optimally estimating the state of the system, which may be partially observed, under the constrain that the system evolves in time according to a linear equation and the observations are linked to the state through a linear equation also.

The script allows to apply Kalman filtering in a simple problem : estimating the state (position and velocity) of an object thrown in the gravity field. Only the position is observed and these observations are perturbed by a random white noise. Therefore, the state made of the 2D position and velocity \(\vec{s} = (\vec{p}, \vec{v})\) evolves according to the following discrete time linear system:

\[\begin{split}\begin{cases} \vec{v}(t+1) &= \vec{v}(t) + \delta t. \vec{g}\\ \vec{p}(t+1) &= \vec{p}(t) + \delta t. \vec{v}(t) \end{cases}\end{split}\]

Suppose a frame of reference with an x-axis rightward and a y-axis upward. Suppose also that we only observe the position. We can write our discrete time evolution and observation functions as:

\[\begin{split}\begin{eqnarray} \begin{bmatrix} x_k \\ y_k\\ \dot{x}_k\\ \dot{y}_k \end{bmatrix} &=& \begin{bmatrix} 1 & 0 & \delta t & 0 \\ 0 & 1 & 0 & \delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} . \begin{bmatrix} x_{k-1} \\ y_{k-1}\\ \dot{x}_{k-1}\\ \dot{y}_{k-1} \end{bmatrix} + \begin{bmatrix} 0 \\ 0\\ 0\\ - g \delta t \end{bmatrix} + \vec{w}_k \\ \rightarrow \vec{s}_k &=& \textbf{A}_k \vec{s}_{k-1} + \vec{u}_k + \vec{w}_k\\ \vec{z}_k &=& \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \end{bmatrix}. \begin{bmatrix} x_k \\ y_k\\ \dot{x}_k\\ \dot{y}_k \end{bmatrix} + \vec{v}_k\\ \rightarrow \vec{z}_k &=& \textbf{H}_k \vec{s}_k + \vec{v}_k \end{eqnarray}\end{split}\]

The evolution and observation functions are linear. Kalman filters are therefore suitable for estimating the state made of the position and velocity. For simulating the filter, we start with an estimate of the position as \(\vec{p}_0 = (0,5)\) and velocity \(\vec{v}_0 = \vec{0}\). The true initial position is \(\vec{p} = (0,0)\) and its initial velocity is 10 \(m.s^{-1}\) at 45 degrees. The kalman filter integrates the observations every 10 ms but we use a simulator with a finer time step of 1 ms. The true observations are perturbed with a white noise of standard deviation 0.8. The object has a mass of 0.1 kg and the gravity is set to 9.81 \(m.s^{-2}\). We simulate 2s. .The simulation was done with the script kalman_ex1.py. Below we show the results of the true state (red line), the observations (green dots) and the estimate (blue dots+line). The blue ellipses indicate the uncertainty on the estimates.

../_images/kalman-ex1.png

Extended Kalman Filters

If the evolution and observation are non-linear, we can linearize them using their Jacobian and transform them into linear equations suitable for a Kalman filter. This 1st order linearization may be too coarse, and this is one motivation for Unscented Kalman Filters we mention in the last section. For now , we will simulate the Extended Kalman Filter on the following non linear system:

\[\begin{split}\begin{eqnarray} \dot{x}(t) &=& \frac{2}{1 + \exp(-y(t) + 1)} - 1 \\ \dot{y}(t) &=& -0.1 x(t)\\ x(0) &=& 1.0 \\ y(0) &=& 0.0 \end{eqnarray}\end{split}\]

The system is simulated with Euler and a time step of \(\delta t = 5. 1e^{-4}\). The EKF receives observations every 0.5 s. We simulate the system for 30s. and therefore the filter receives 60 observations. The initial estimates are set to \(0, 0\). Below we plot, on the left the true state and on the right the observations and the EKF estimates.

../_images/ekf-ex1.png

The initial thoughts behind this example was to manipulate the stiffness of \(y(t)\) to oblige the sigmoid to enter the non-linear part of the sigmoid and to check how the EKF behaves but I did not do so for the moment.

Unscented Kalman Filters

Unscented Kalman filters handle also non-linear evolution and observation functions. However, contrary to EKF, it does not involve linearizing the evolution/observation functions but rather it directly estimates how the first two moments of the state (a random variable) change through these non-linear functions. It relies on the Unscented Transform for this update of the mean and covariance of the random variable.

Soon or later! This is in the todo list. But check out the ** `easykf`_ **project page for some examples using the UKF.