Kalman Filters

Kalman Filter algorithms (EKF,IEKF,UKF) are centralized in one single virtual class, mrpt::bayes::CKalmanFilterCapable. This class contains the system state vector and the system covariance matrix, as well as a generic method to execute one complete iteration of the selected algorithm. In practice, solving a specific problem requires deriving a new class from this virtual class and implementing a few methods such as transforming the state vector through the transition model, or computing the Jacobian of the observation model linearized at some given value of the state space.

1. Kalman Filters in the MRPT

Kalman Filter algorithms (EKF,IEKF,…) are centralized in one single virtual class, mrpt::bayes::CKalmanFilterCapable. This class contains the system state vector and the system covariance matrix, as well as a generic method to execute one complete iteration of the selected algorithm. In practice, solving a specific problem requires deriving a new class from this virtual class and implementing a few methods such as transforming the state vector through the transition model, or computing the Jacobian of the observation model linearized at some given value of the state space.

A set of parameters that are problem-independent can be changed in the member KF_options of this class, where the most important parameter is the selection of the KF algorithm. Currently it can be chosen from the following ones:

  • Naive EKF: The basic EKF algorithm.
  • Iterative Kalman Filter (IKF): This method re-linearizes the Jacobians around increasingly more accurate values of the state vector.

This class has been used to implement an efficient solution to 6D-SLAM.

Another implementation of Bayesian filtering in the MRPT C++ library are Particle Filters.

2. Writing a KF class for a specific problem

2.1. Deriving a new class

First a new class must be derived from mrpt::bayes::CKalmanFilterCapable. It should be declared a public method as an entry point for the user, which saves the input data (observations,actions,…) and calls the protected method runOneKalmanIteration() of the parent class.
There are two fundamental types of systems the user can build by deriving a new class:

  • A normal problem: The size of the state vector remains unchanged over time. This size must be returned by the virtual method “get_vehicle_size()”.
  • A mapping-like problem: The size of the state vector grows as new map elements are incorporated over time. In this case the first “get_vehicle_size()” elements of the
    state vector correspond to the state of the vehicle/robot/camera/… and the rest of the state vector is a whole number of “get_feature_size()” sub-vectors, each describing one map element (landmark, feature,…).

2.2. The internal flow of the algorithm

The method mrpt::bayes::CKalmanFilterCapable::runOneKalmanIteration() will sequentially call each of the virtual methods, according to the following order:

  • During the KF prediction stage:
    • OnGetAction.
    • OnTransitionModel.
    • OnTransitionJacobian.
    • OnTransitionNoise.
  • OnNormalizeStateVector: This can be optionally implemented if required for the concrete problem.
  • OnGetObservations: This is the ideal place for generating observations in those applications where it requires an estimate of the current state (e.g. in MonoSLAM, to predict where each landmark will be found onto the image). At this point the internal state vector and covariance contain the “prior distribution”, i.e. updated through the transition model. This is also the place where data association must be solved (in mapping problems).
  • During the KF update stage:
    • OnObservationModelAndJacobians: This method will be called just once in naive EKF, or several times in the EKF a la Davison.
  • OnNormalizeStateVector.
  • If the system implements a mapping problem, and the data association returned by “OnGetObservations” indicates the existence of unmapped observations, then the next method will be invoked for each of these new features:
    • OnInverseObservationModel.
  • OnPostIteration: A placeholder for any code the user wants to execute after each iteration (e.g. logging, visualization,…).

3. An example

An example of a KF implementation can be found in the samples directory (
samples/bayesianTracking) for the problem of tracking a vehicle from noisy range and bearing data. Here it will be derived the required equations to be implemented, as well as how they are actually implemented in this example. Note that this problem is also implemented as a Particle Filter in the same example in order to visualize side-to-side the performance of both approaches.

3.1. Problem Statement

The problem of range-bearing tracking is that of estimating the vehicle state:

\mathbf{x}=(x ~ y ~ v_x ~ v_y)

where x and y are the Cartesian coordinates of the vehicle, and vx and vy are the linear velocities. Thus, we\’ll use a simple constant velocity model, where the transition function is:

\mathbf{x_k} = f(\mathbf{x_{k-1}}, \Delta t) = \left\{ \begin{array}{l} x_{k-1} + v_{x_k} \Delta t \\ y_{k-1} + v_{y_k} \Delta t \end{array} \right.

We will consider the time interval between steps ?t as the action u of the system. The observation vector $z=(z_b ~ z_r)$latex consists of the bearing and range of the vehicle from some point (arbitrarily set to the origin):

z_b = atan2(y,x) z_r = \sqrt{ x^2 + y^2 }

Then, it is straightforward to obtain the required Jacobian of the transition function:

\frac{\partial f}{\partial x} = \left( \begin{array}{cccc} 1 ~ 0 ~ \Delta t ~ 0 \\ 0 ~ 1 ~ 0 ~ \Delta t \\ 0 ~ 0 ~ 1 ~ 0 \\ 0 ~ 0 ~ 0 ~ 1 \end{array} \right) latex

and the Jacobian of the observation model:

\frac{\partial h}{\partial x} = \left( \begin{array}{cccc} \frac{-y}{x^2+y^2} ~ \frac{1}{x\left(1+\left( \frac{y}{x} \right)^2\right)} ~ 0 ~ 0 \\ \frac{x}{\sqrt{x^2+y^2}} ~ \frac{y}{\sqrt{x^2+y^2}} ~ 0 ~ 0 \end{array} \right)

3.2 Implementation

The most important implemented methods are detailed below. For further details refer to the source code of the “bayesianTracking” example.

3.2.1 The transition model

The constant-velocities model is implemented simply as:

3.2.2 The transition model Jacobian

3.2.3 The observations and the observation model

3.3 Results

The following video shows the example samples/bayesianTracking running:

Leave a Reply

Your email address will not be published. Required fields are marked *

*

You may use these HTML tags and attributes: <a href="" title=""> <abbr title=""> <acronym title=""> <b> <blockquote cite=""> <cite> <code class="" title="" data-url=""> <del datetime=""> <em> <i> <q cite=""> <strike> <strong> <pre class="" title="" data-url=""> <span class="" title="" data-url="">

Search C++ API:

 

Share

Latest blog entries

Recent activity

Stats

stats
(more)

web stats