Since our AHRS hardware has theoretically close to desktop-class performance on math-intensive code, computational burden wasn't much of a limiting factor in deciding which attitude estimation approach to use. Over the previous couple of years Daniel had done significant research into the state of the art with regard to attitude estimation, and decided that an Unscented Kalman Filter (UKF) was the best fit for our requirements.

The main advantages of the UKF over the Extended Kalman Filter (EKF) are the ability to deal with arbitrary non-linear dynamics models without requiring analytical derivatives or other challenging-sounding stuff, and the (theoretical) better stability in the presence of un-modelled non-linearity.

Again, since computational burden wasn’t much of an issue, we decided to use a 22-element state vector, containing position (lat/lng/alt, since it makes dealing with GPS altitude error easier), velocity (NED), acceleration (body frame), attitude (quaternion), angular velocity, angular acceleration, gyro bias (3 dimensions) and wind velocity (NED). Acceleration and angular acceleration are often not included in Kalman filter state vectors, but in this case it makes some of the processing more elegant so we’re likely to leave them in unless there are significant disadvantages.

Daniel implemented the filter in C++ based on the Eigen library, and with Python bindings it runs approx 5× real time on a 2.7GHz i7, albeit without the flight dynamics model enabled. The DSP should be powerful enough to run it as-is with the dynamics model on a single core, leaving the other core for the NMPC algorithm.

Main learnings from filter implementation were around the specific choices of algorithms to use: there are several variations, and some have significantly better stability on our datasets than others. Specifically:

- Quaternion mean finding and error calcualations use Modified Rodriguez Parameters (MRPs), which result in much more stable attitude code than the various iterative or heuristic-based mean finding approaches.
- Using the Scaled Unscented Transform is critical; the “best” values for
α, β and κ were 1.0, 0.0 and 3.0 respectively. One potential area for
improvement is using one of the sigma point calculation methods that doesn’t
require the full 2
*n*+1 points; since the dynamics model has to be evaluated once at each sigma point this would be a fairly significant win. - Cholesky decomposition is basically fine with the above sigma point
scaling parameters; before using them, we used the more-commonly-recommended
α ≈ 10
^{-3}, β = 0, κ = something negative, and the covariance matrix frequently ended up non-positive-definite. - ECEF position seems to yield slightly better accuracy under ideal
circumstances, but the problem is that it’s difficult to decouple the rubbish
GPS altitude estimates from the fairly good horizontal position estimates
using ECEF, since each component has an effect on all three axes. Switching to
lat/lng/alt makes that much easier, so we can get decent horizontal position
results while pretty much ignoring GPS completely for altitude (unless
*e.g.*the barometric pressure sensor fails).

The “throw everything at the filter and see what happens” approach is nice
in a lot of ways; for example, it means no special-case code needs to be
written for the magnetometer, since you're just predicting the measured
field vector based on the World Magnetic Model
field vector and the current attitude. The wind vector, likewise, needs no
special effort to maintain—it just gets worked out based on velocity, airspeed
and attitude. There's also no need to consider specific sensor interactions,
and normally the sensor covariances are determined by the measured noise and
static error bounds (*e.g.* axis misalignment).

Basic validation of the filter (particularly the kinematics and sensor models) was done with a set of unit tests; more extensive validation was done using 100Hz X-Plane data recordings with simulated sensor latency and reduced update frequencies for position and velocity.

The filter source code is available at sfwa/ukf.