-
Notifications
You must be signed in to change notification settings - Fork 173
Python API
pip install https://github.com/sfwa/ukf/archive/master.zip#egg=ukf-1.0.0
Dependencies should be installed automatically.
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++/Eigen3Or:
ukf.init(implementation="c66x") # C66x DSP implementation in pure CThe 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 aircraftOr:
ukf.model = ukf.UKF_MODEL_CENTRIPETAL # Basic centripetal dynamics modelBy 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 depends on the quality of the dynamics model (or the reasonableness of not modelling dynamics); the values below are a sane starting point for a 1000Hz update rate, but tuning will be required (also, if you're varying the update rate the process noise values will need to be varied accordingly: this is NOT automatic!). 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. 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)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
Note however that running the UKF with a variable step size (the first parameter to ukf.iterate(…)) 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.