Skip to content

Home

Jump to bottom
Ben Dyer edited this page Nov 1, 2013 · 5 revisions

The Unscented Kalman Filter (UKF) is a combination of the Unscented Transform with the Kalman filter, designed for use with non-linear systems. Its handling of non-linearity is generally better than that of the Extended Kalman Filter, although it is more computationally intensive.

This library provides two implementations of the same UKF, one written in C++ using the Eigen 3 library, and one written in plain C optimised for the Texas Instruments Keystone cores found in the C66x series of DSPs.

See our development log for general background information.

Usage

The UKF can be called from Python or C/C++. If using with Python, just run:

pip install https://github.com/sfwa/ukf/archive/master.zip#egg=ukf-1.0.0

to build and install. See the Python documentation for an overview of the API.

If using with C or C++, first build the C++ static library with CMake as follows:

git clone https://github.com/sfwa/ukf.git
mkdir ukf_build && cd ukf_build
cmake ../ukf
make

Then the C dynamic library wrapper can be built:

make cukf

The best source of API documentation for these libraries is the C/C++ source code (sorry!).

Filter design

The filter maintains a 25-element state vector containing an estimate of the following values:

  • Position: latitude (radians), longitude (radians), altitude above WGS84 ellipsoid (metres);
  • Velocity: north, east, down (m/s) with the coordinate system centred on the current position;
  • Acceleration: body frame x/forward, body frame y/starboard, body frame z/down (m/s^2);
  • Attitude: quaternion (w, x, y, z);
  • Angular velocity: x/rolling starboard, y/pitching up, z/yawing starboard (rad/s);
  • Angular acceleration: x, y, z (rad/s^2)
  • Wind velocity: north, east, down (m/s) with the coordinate system centred on the current position;
  • Gyro bias: x, y, z (rad/s).

The filter's measurement model can be altered by subclassing SensorModel, but the default model supports the following sensors:

  • Accelerometer: x, y, z (m/s^2);
  • Gyroscope: x, y, z (rad/s);
  • Magnetometer: x, y, z (µT);
  • GPS position: latitude (radians), longitude (radians), altitude above WGS84 ellipsoid (metres);
  • GPS velocity: north, east, down (m/s);
  • Pitot true airspeed measurement: m/s along the positive body-frame x-axis;
  • Barometer: altitude above WGS84 ellipsoid (metres).

Note that the pitot and barometer sensor models assume that the raw readings have been converted into true airspeed and altitude before being passed to the UKF.

The accelerometer, gyroscope and magnetometer sensor models allow an orientation quaternion to be saved for each sensor, describing the sensor's relationship to the body frame. The accelerometer sensor model also allows an offset from the vehicle's centre of mass to be set, and that will automatically be factored in to estimates of acceleration while rotating.

The sensor model also requires a local magnetic field vector to be provided (in µT, NED coordinate system). This must be calculated by external code using the WMM/EMM or another appropriate magnetic model.

The UKF implementations largely follow A Quaternion-based Unscented Kalman Filter for Orientation Tracking by Edgar Kraft, with these modifications:

  • Due to the large state vector (25 elements) Simon Julier's Scaled Unscented Transform is used to keep the sigma point distribution reasonably small.
  • Modified Rodrigues parameters (MRPs) are used for quaternion averaging as described in Unscented Filtering for Spacecraft Attitude Estimation by John L. Crassidis and F. Landis Markley.
  • The measurement estimate/innovation code handles variably-sized measurement vectors, since not all sensors provide data every filter update.
Clone this wiki locally