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:

ukf.model = ukf.UKF_MODEL_FIXED_WING  # Flight dynamics model for fixed-wing aircraft

Or:

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. 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 depends on the quality of the dynamics model (or the reasonableness of not modelling dynamics); the values below are a sane starting point, but tuning will be required. Lowering the value will make the filter trust the corresponding model outputs more, and increasing the value will make the filter trust them less.

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 is left as an exercise for the reader.

Having completed initialisation and configuration, the UKF initial state can be set.

TODO