Python API

Installation

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

Dependencies should be installed automatically.

Module usage

First:

import ukf

Now, select the underlying UKF implementation. Two are currently defined; one written in C++ using Eigen 3, and the other in somewhat DSP-optimised C. Both are built by default, and yield very similar (but not identical) results.

ukf.init(implementation="c")  # C++/Eigen3

Or:

ukf.init(implementation="c66x")  # C66x DSP implementation in pure C

The default sensor model requires some configuration before measurements can be passed in. All vectors are 3-tuples: (x, y, z) for sensor offsets, angular velocity, angular acceleration, and linear acceleration; (n, e, d) for velocity and magnetic field strength; (lat, lon, alt) for position.

ukf.configure_sensors(
    # Accelerometer position and orientation in the body coordinate 
    # system, with the origin at the centre of mass
    accelerometer_offset=(0, 0, 0),          # x, y, z (metres)
    accelerometer_orientation=(1, 0, 0, 0),  # w, x, y, z (quaternion)
    # Gyroscope orientation relative to the body frame
    gyroscope_orientation=(1, 0, 0, 0),      # w, x, y, z (quaternion)
    # Magnetometer orientation relative to the body frame
    magnetometer_orientation=(1, 0, 0, 0),   # w, x, y, z (quaternion)
    # Magnetic field at the current/starting position, as calculated
    # by a suitable magnetic field model
    wmm_field=(21.2578, 4.4132, -55.9578),   # n, e, d (µT)
    # Accelerometer covariance (square of expected noise/error)
    accelerometer_covariance=81.0,           # (m/s^2)^2
    # Gyroscope covariance (square of expected noise/error)
    gyroscope_covariance=math.radians(10) ** 2,  # (rad/s)^2
    # Magnetometer covariance (square of expected noise/error), 
    magnetometer_covariance=1.5,             # µT^2
    # GPS position covariance (square of expected noise/error, 
    # including latency)
    gps_position_covariance=(1e-12, 1e-12, 225.0),  # (rad^2, rad^2, m^2)
    # GPS velocity covariance (square of expected noise/error, 
    # including latency)
    gps_velocity_covariance=(9.0, 9.0, 49.0),  # (m/s)^2
    # Pitot true airspeed covariance (square of expected noise/error)
    pitot_tas_covariance=100.0,                # (m/s)^2
    # Barometer altitude covariance (square of expected noise/error)
    barometer_amsl_covariance=4.0              # m^2
)

The UKF can use a dynamics model to make up for some level of sensor inaccuracy. There are two models included in the library: a basic centripetal model (accounting for the effects of turning while moving on measured acceleration), and a fixed-wing flight dynamics model.

To set the dynamics model, call e.g.:

ukf.model = ukf.UKF_MODEL_CENTRIPETAL  # Basic centripetal dynamics model

By default, ukf.model is ukf.UKF_MODEL_NONE, i.e. no dynamics model is used. This page covers the available dynamics models. It's probably better to use no dynamics model at all rather than one that is not designed for your intended application.

Following that, the UKF's process noise needs to be set. This noise represents the "uncertainty" (in the form of variance) added to the UKF's state estimate each timestep. The optimum amount of noise depends on the quality of the dynamics model, and/or the reasonableness of not modelling dynamics. Lowering these values will make the filter trust the corresponding model outputs more, and increasing the value will make the filter trust them less.

Since the values below are added to the state each update timestep, if you're varying the update rate the process noise values will need to be varied accordingly. The values below are sane for a fairly good dynamics model and a timestep of 1000Hz; your mileage will vary.

ukf.configure_process_noise((
    1e-14, 1e-14, 1e-4,  # Position covariance: lat, lon, alt
    7e-5, 7e-5, 7e-5,    # Velocity covariance: n, e, d
    2e-4, 2e-4, 2e-4,    # Acceleration covariance: body x, y, z
    3e-9, 3e-9, 3e-9,    # Attitude covariance: yaw, pitch roll
    2e-3, 2e-3, 2e-3,    # Angular velocity covariance: x, y, z
    1e-3, 1e-3, 1e-3,    # Angular acceleration covariance: x, y, z
    1e-5, 1e-5, 1e-5,    # Wind velocity covariance: n, e, d
    3e-12, 3e-12, 3e-12  # Gyro bias covariance: x, y, z
))

If the fixed-wing dynamics model is used (ukf.model is ukf.UKF_MODEL_FIXED_WING), airframe configuration parameters can be supplied as follows.

ukf.configure_airframe(
    # Aicraft mass in kg
    mass=3.8,
    # Aircraft inertia tensor (metric -- metres/kilograms/seconds)
    inertia_tensor=(
        2.59e-1, 0, -0.334e-1,
        0, 1.47e-1, 0,
        -0.334e-1, 0, 4.05e-1
    ),
    # 
    prop_coeffs=(0.025, 0.00250),
    # Lift force coefficients: x[0] * alpha^4 + x[1] * alpha^3 + x[2] * alpha^2 + x[3] * alpha + x[4]
    lift_coeffs=(-3.7, -5.4, 1.3, 1.7, 0.05),
    # Drag force coefficients: x[0] * alpha^4 + x[1] * alpha^3 + x[2] * alpha^2 + x[3] * alpha + x[4]
    drag_coeffs=(0.0, 0.0, 0.5, 0.0, 0.005),
    # Side force coefficients: x[0] * beta^2 + x[1] * beta + x[2] * yaw rate + x[3] * roll rate + x[4:8] * control[0:4]
    side_coeffs=(
        0, -2.35e-01, -1.87e-03, 4.53e-04,
        0.0, 1.1e-02, -1.1e-02, 0.0
    ),
    # Pitch moment coefficients: x[0] * alpha + x[1] * (pitch rate)^2 * sign(pitch rate) + x[2:6] * control[0:4]
    pitch_moment_coeffs=(-0.001, -0.014, 0.0, -0.03, -0.03, 0.0),
    # Roll moment coefficients: x[0] * roll rate + x[1:5] * control[0:4]
    roll_moment_coeffs=(-0.002, 0.0, -0.03, 0.03, 0.0),
    # Yaw moment coefficients: x[0] * beta + x[1] * yaw rate + x[2:6] * control[0:4]
    yaw_moment_coeffs=(0, -0.005, 0.0, 0.001, 0.001, 0.0)
)

Selection of these parameters (or development of alternative dynamics models) is left as an exercise for the reader.

Having completed initialisation and configuration, the UKF initial state can be set. All elements in the state vector default to 0, except for the initial attitude (which is (1, 0, 0, 0)).

# Set position (lat in radians, lon in radians, alt in metres)
ukf.state.position = (math.radians(27.174526), math.radians(78.042153), 50.0)

# Other values can be set as well
ukf.state.velocity = (0, 0, 0)
ukf.state.acceleration = (0, 0, 0)
ukf.state.attitude = (1, 0, 0, 0)
ukf.state.angular_velocity = (0, 0, 0)
ukf.state.angular_acceleration = (0, 0, 0)
ukf.state.wind_velocity = (0, 0, 0)
ukf.state.gyro_bias = (0, 0, 0)

If possible it's good to set the initial state to something reasonably near the vehicle's actual state. For example, if you have a GPS fix and magnetometer heading available, feeding that data in will be close enough for the UKF to converge quickly. In particular, having the filter position on the other side of the world from the vehicle's actual position may cause problems with convergence; the lat/lon update code is written for efficiency in small movements, and is not expected to be correct for changes in position over more than a small fraction of the earth's circumference.

At this point your program will typically enter a loop of some kind, waiting for measurement data to be received. There are two main ways this can be approached: either poll for sensor data while running the filter at a constant rate, or block while waiting for sensor data and then run the filter enough to catch up based on the time delta.

Polling method:

while True:
    …  # Look for sensor data etc
    if sensor_data_available:
        ukf.set_sensors(**sensor_data)
    ukf.iterate(0.001, control_values)

Blocking method:

t_start = time.time()
while True:
    sensor_data = await_sensor_data()
    ukf.set_sensors(**sensor_data)
    t_end = time.time()
    ukf.iterate(t_end - t_start, control_values)
    t_start = t_end

The first parameter to ukf.iterate(…) is the time delta (step size), in seconds. This can be zero, if you want to add extra sensor data (e.g. multiple accelerometer readings) for a single time period.

Note however that running the UKF with a variable step size is not particularly well supported; you'll need to adjust the process noise covariance (ukf.configure_process_noise(…)) for each step and there may be other issues as well.

In the above examples, sensor_data is expected to be a dictionary like this (all keys are optional):

sensor_data = {
    "magnetometer": (x, y, z),
    "accelerometer": (x, y, z),
    "gyroscope": (x, y, z),
    "gps_position": (lat, lon, alt),
    "gps_velocity": (n, e, d),
    "barometer_amsl": alt,
    "pitot_tas": airspeed
}

The control_values vector is a 4-tuple that contains control surface positions, throttle values or other things that make sense to the selected dynamics model.

After running ukf.iterate(…), ukf.state is updated with the current state estimate. You don't need to update all (or indeed any) sensor measurements each iteration, so it's possible to mix sensors with different data rates or timing as required. For example, the accelerometer and gyro readings might be available at 1000Hz, while the barometer arrives at 300Hz, the magnetometer at 100Hz and the GPS at 10Hz.