[sfwa]

TRICAL — easy magnetometer calibration

Magnetometers are tricky. Aside from the usual set of sensor issues to overcome—axis misalignment, scale factor error, bias error, and thermal effects on bias—the field they’re measuring varies significantly in direction and intensity around the world, and is subject to orientation-dependent influence from various metals, electric currents, and permanent magnets.

The complexity of the possible errors makes it essentially impossible to calibrate a magnetometer without it being installed, and even then it’s unlikely to provide good results without frequent re-calibration. (This is one of the reasons you have to wave your iPhone around in circles every time you want to use the compass.)

Since we need to be able to recalibrate the magnetometer during flight, we want to avoid batch-mode algorithms. Although they’re good for ground use (or pre-flight use), if the results differ too significantly from one calibration run to the next, the discontinuities in calibrated magnetometer output could cause instability in the UKF.

Fortunately there are a variety of algorithms that allow a magnetometer to be calibrated during use. These range in complexity from simple centering approaches using linear approximations (essentially averaging the readings out to determine the bias) through to the current gold standard, TWOSTEP (which is a batch approach, but is sufficiently stable to re-apply periodically). Since spacecraft have been using triaxial magnetometers much longer than small UAVs, have most of the published approaches are robust and efficient to implement, although generally run over much longer timescales and assume much more accurate sensors.

The simpler approaches have a few drawbacks, the most significant of which is an inability to converge on a solution for certain combinations of sensor error and field distortion. That could lead to instability in the output of the calibration algorithm, which would be fairly disastrous for the assumptions the UKF makes about error (specifically, white Gaussian).

The TWOSTEP algorithm looks nice, and has been successfully used on spacecraft. OpenPilot have an Eigen-based implemenation, but they’re not doing in-flight re-calibration, and the complexity around integration of a batch system within the AHRS UKF counted against it. The algorithm is also relatively performance-intensive, and while we’re below 50% on our CPU budget for the UKF core, I don’t want to use up a significant chunk of that on magnetometer calibration updates.

So, we turned to Real-Time Attitude-Independent Three-Axis Magnetometer Calibration by Crassidis, Lai and Harman (Crassidis being the author of a number of papers we referred to while developing the UKF). This paper implements a simple linear centering algorithm, an EKF-based algorithm, a UKF-based algorithm and TWOSTEP, and compares them on simulated and actual satellite datasets. The results show that TWOSTEP is pretty close to optimal in almost all cases, while the centered approach is not too bad but can result in alarming errors every now and then. The EKF is normally better than the centered approach, but showed significant divergence when tested with coloured noise; the UKF results were pretty much indistinguishable from the TWOSTEP results.

Since a UKF approach seems to work well for this type of problem, and we were already familiar with UKF implementation, we decided to go with that for our real-time calibration system. The result is TRICAL, a simple calibration solution for tri-axial field sensors.

TRICAL uses a 9-state scaled formulation of the UKF, with a scalar measurement model (basically the magnitude of the field). The state vector contains the X, Y and Z bias values (accounting for sensor bias and hard iron distortion), as well as a symmetric 3×3 matrix of scale factors (accounting for sensor scale factor error, non-orthogonality, and soft iron distortion). Since the scale factor matrix is symmetric, we only need to store 6 elements rather than the full 9.

The relatively small state vector, as well as the entirely absent process model and simple measurement model make this UKF much less computationally intensive than the one we use for our AHRS—something like 10,000 cycles per iteration compared with 250,000–450,000 cycles per iteration. We’re running two TRICAL instances, one for each magnetometer, and updating them every time we get a magnetometer reading (~110Hz × 2). (This isn’t strictly necessary, as applying the calibration estimate to a measurement doesn’t require that the filter be run, but it keeps CPU utilisation predictable.)

To visualise the calibration results, I wrote a Python script that runs TRICAL over a set of magnetometer readings in a CSV, and outputs a WebGL visualisation as a single (large) HTML file. An example can be viewed here (6.6MiB; WebGL-capable browser required). Results look much cleaner than the input data, and although we don’t have data for the full set of attitudes (no inverted flight that time), the calibrated data points do look spherical despite wildly varying sensor error over the course of the flight.

Screenshot of TRICAL’s WebGL output

I also integrated the TRICAL Python interface with our test flight viewer script, replacing the previous manually-derived calibration parameters. There was an immediate small improvement in accuracy, but when I adjusted the covariance values to account for the improvement, the result was significant:

Model ° Error at Percentile
Pitch Roll
50th 75th 95th RMS 50th 75th 95th RMS
None 4.0 6.6 13.4 6.5 4.5 7.8 14.6 7.3
Centripetal 2.6 4.4 9.3 5.4 3.0 5.4 12.8 7.7
Fixed-wing 1 2.2 3.7 7.2 4.3 2.7 4.7 8.3 6.1
Fixed-wing 2 2.4 3.8 6.4 3.4 2.2 3.7 6.1 3.3
X8 + TRICAL 1.8 3.1 5.9 2.9 1.7 2.9 5.0 2.7

The plot of baseline vs estimated pitch is available here, and roll here. I suspect the baseline pitch values contain scale factor error due to camera calibration issues, so it's likely the true results are a bit better than depicted. Accuracy of baseline roll is (visually) something like ±2° RMS.

Next up is accelerometer bias and scale factor calibration, as well as gyro scale factor calibration (gyro bias is already estimated by our AHRS UKF).

The TRICAL source code, including Python interface and batch calibration script, is available at sfwa/TRICAL.

Usage from C is very straightforward:

#include "TRICAL.h"

TRICAL_instance_t global_instance;

/* ... */

void your_init_proc(void) {
    /* ... */

    TRICAL_init(&global_instance);
    TRICAL_norm_set(&global_instance, 60.0);
    TRICAL_noise_set(&global_instance, 1.5);
}

void your_sensor_read_proc(void) {
    float sensor_reading[3];

    /* ... */

    TRICAL_estimate_update(&global_instance, sensor_reading);

    /* ... */

    float calibrated_reading[3];
    TRICAL_measurement_calibrate(&global_instance, sensor_reading,
                                 calibrated_reading);

    /* Now use calibrated_reading as an input to your AHRS or whatever */
}

And from Python it’s even simpler:

import TRICAL

instance = TRICAL.Instance(field_norm=60.0, measurement_noise=1.5)

for sensor_reading in set_of_readings:
    # sensor_reading is a sequence containing 3 elements: (X, Y, Z)
    instance.update(sensor_reading)

    calibrated_reading = instance.calibrate(sensor_reading)
github.com/sfwa twitter.com/sfwa_uav youtube.com/user/sfwavideo