Sort by

### Yet another AHRS algorithm

Although I have been here for years, this is my first blog post and I would like to present yet another attitude estimator which I have been working in my M.Sc. Thesis. A friend of mine and me have developed an autopilot, but this is for another post. However you can find details about it in the thesis at the end of the post. For our tests we are using a Mentor from Multiplex, shown in the next figure.

The AHRS is based on an Unscented Kalman Filter, which has better performance than the classical Extended Kalman Filter for non-linear systems, such as the kinematic equation for attitude.
This algorithm is based on three axis magnetometers, three axis accelerometers and three axis gyroscopes, so it can estimates the yaw angle at low/none velocity without the GPS aid. As other filters, it uses the accelerometers and magnetometers to estimate and neutralize the biases of the gyroscopes, which estimate the attitude angles in quaternion form. The difference with other algorithms is that the measurements from the accelerometers and/or magnetometers are not put directly in the filter, but they are indirectly put through an algorithm called Fast Optimal Attitude Matrix (FOAM) as it is shown in the next figure (sorry for the Spanish).

FOAM builds the Direct Cosine Matrix (DCM) from the accelerometers and magnetometers readings allowing to weight the sensors, thus in critical stages such as take-off or "aerobatics", we can rely more in magnetometers readings than in accelerometers for correcting the gyroscope's integration in an autonomous way.

I have validated the algorithm using GPS readings for the yaw angle, despite the cross-wing during a turn (10-20secs, the wind speed was around 15m/s this day as you can guess from the video) the yaw and heading angle are closer. The roll angle was validated with a camera on board in the same experiment. The details of the vision algorithm, shown in the video, can be found in the thesis. In simulation (using X-Plane or mathematical model in Simulink), comparing with true values, the error is less than 1º in pitch and roll and 4º in yaw once the filter has converged.

Mentor allows me for automatic taking-off with a robust landing gear. It is nice to see how the AHRS works very well in presence of many vibrations during this stage. Look at the next figures how the trust in accelometers (red line) downs during the take-off moment and rises again when the flight is going to steady, not disturbing the pitch correction in any moment. As well, the algorithm converges very fast to true values (6º of pitch at the moment of take-off).

You can download the master thesis (in Spanish), detailing the algorithm and the hardware/software here: ﻿﻿﻿﻿http://dl.dropbox.com/u/2689187/tesina_Hector.pdf

The next article is a resume (in English) of the related algorithm using TRIAD (a simplified version of FOAM). It will be published in the special issue about Kalman Filter Issues in Transactions on Industrial Electronics Journal within next two months, so if you want to cite it, you have to search it there: http://dl.dropbox.com/u/2689187/article_TIE.pdf

Here is a nice resume of several algorithms to estimate the DCM, it includes TRIAD and FOAM among other ones:

http://dl.dropbox.com/u/2689187/dcm_estimation.pdf

If you have questions, fell free to ask :P