Kalman filter

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:

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.

UKF output based on X-Plane data logs, indicating some tuning is required
github.com/sfwa twitter.com/sfwa_uav youtube.com/user/sfwavideo