openAHRS: Extended vs Unscented Kalman Filter

I've been thinking on openAHRS lately. Some days ago I found Sean D'Epagnier in FreeNode's #avr channel (IRC) and we started talking about his project and openAHRS, regarding sensor calibration and how things could evolve from now on.

Sean mentioned about the Sigma Point Kalman Filter (SPKF) and that it might improve performance for non-linear systems. I started searching for

papers on the Unscented Kalman Filter (UKF) and other information related to it.

After some tests I decided to compare the UKF with the EKF (Extended Kalman Filter) to see how good improvements are, and here are the results.

All input data was measured from the AVR32 openAHRS port. There are no precise calibrations, only some minor magnetometer ones and nothing else. The data was exported and UKF and EKF were implemented in Matlab.

The filters were tested under three different cases by exciting each rotation angle independently (or at least as far as independent as my hand could do it). There are peaks in the raw angles which are measured from accelerometer data, since a big deacceleration happened when I hit the table (on purpose) when I was getting close to 0 degrees.

I forgot to add the axis labels, but angles are shown in radians on the y-axis and time (in samples at 50Hz) is on the x-axis.

Results of exciting the Roll axis

Both filters did well: EKF / UKF. UKF did converge faster to a true value than EKF. I have to mention that the noise covariance matrices were the same for both filters.

Results of exciting the Pitch axis

Something similar to the case above happens. Here is the EKF one and there the UKF. It seems that I've disturbed the other axis I shouldn't have modified.

Results of exciting the Yaw (heading) axis

The yaw angle input is not affected from acceleration, at least not if the predicted pitch and roll are precise enough. This happens because yaw is calculated by using magnetic field sensing, so it will be quite accurate compared to the accelerometer readings. Here are the EKF results and here the UKF ones.

The EKF is a mess and the UKF doesn't do so well either. This has a simple explanation: lack of magnetometer calibration. I'll be adding some suggested code by Sean to see if it improves.

EKF and instability

The serious problem with EKF is instability. When playing around with the noise variance matrices (both measurement and process noise) there are certain points where the filter loses stability.

It's very important to let the states and noise matrices adapt to the system and noises before perceptible movements are applied to the sensors. A way to avoid this is to save the state covariance matrix P so that the kalman filter only needs a small time to adapt once powered up.

All these concepts apply to UKF except that it is much more stable. I was able to 'play' with the noise matrices in a free way. It's important to notice that UKF converges much faster than EKF at startup, probably because of the second and third order predictions EKF is not able to compute.

Kalman Tuning

The filter won't work without a minimal knowledge of how and what it is doing. It's important to understand what each coefficient in the noise matrices mean. For example, the filter needs to know that it should believe in the gyro bias estimates much more than in the accelerometers so that gyro data becomes credible in short-term measurements so tilt information does the same for long-terms. This also means that the gyro bias estimates will remain practically constant, changing their value slowly and thus adapting to temperature and time drifts.

To do

There is a large ToDo list:

• Test if the EKF can be improved, specially the heading axis.
• Implement SQ-UKF (Square Root KF) and/or UD-UKF since the classic UKF is really slow because of the matrix square root calculation.
• Implement a robust calibration routine for all the sensors. Sean told me some nice ideas that could greatly improve precision.