Unscented Kalman filter for a Differential Drive Robot

Introduction to Unscented Kalman Filters

The Unscented Kalman filter (UKF) attempts to obtain a more accurate linearization by using multiple samples to calculate the covariance. Instead of passing a single point through the action transition function and shaping the covariance, it passes multiple points through the action transition funtion and calculates the covariance from scratch using the transformed points. A primitive version of this filter might use a large number of samples and calculate the covariance and mean of the posterior distribution by brute force. When all of the distributions involved Gaussian, it is possible to construct the posterior Gaussian using a few specifically selected samples. These deterministically selected samples are referred to as sigma points.

The sigma point selection is influenced by three parameters:

Action Update

The UKF requies its action model to be a GaussianActionModel.

The code to set up the filter does not change much from the EKF. Because the UKF models the nonlinearities better, the error bounds grow more quickly than the EKF. To slow the error growth, the amount of error specified by R has been reduced.

DDActionModel actionModel = new DDActionModel();
actionModel.AxleLength = 5;
actionModel.R = new Matrix(3, 3, 0.01, 0, 0, 0, 0.01, 0, 0, 0, 0.01);
UnscentedKalmanFilter<RobotPose2, DifferentialDriveAction, RobotPose2Observation> filter =
    new UnscentedKalmanFilter<RobotPose2, DifferentialDriveAction, RobotPose2Observation>();
filter.Alpha = 0.001;
filter.Beta = 2.0;
filter.Kappa = 0.0;
filter.Belief = new GaussianMoment<RobotPose2>(
    new RobotPose2(200, 50, 0),
    new Matrix(3, 3, 10, 0, 0, 0, 10, 0, 0, 0, 0.01)
filter.ActionModel = actionModel;

The code to update the belief is identical to the EKF.

DifferentialDriveAction u = new DifferentialDriveAction(data.Vl, data.Vr, data.t);

Sensor Update

The sensor update uses the same technique of sampling and covariance calculation as the action update. However, for this tutorial, we will use the same sensor model as we did with the Kalman filter and the EKF. The code to setup the filter and update with the sensor observation is exactly the same.

DDSensorModel sensorModel = new DDSensorModel();
sensorModel.Q = new Matrix(3, 3,
    10, 0, 0,
    0, 10, 0,
    0, 0, 0.1);
filter.SensorModel = sensorModel;

RobotPose2Observation z = GetObservation();

The following Silverlight app shows the UKF in action.

Get Microsoft Silverlight

Differential Drive Tutorial Series:

Copyright 2009 Cognitoware. All rights reserved.