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
Comments
Could you tell about UKF and TRIAD or could you share the code in matlab? if you dont mind.
here is my mail: nuaalq@qq.com
sure, I will contact you by mail
thank you for your respond.
I'll review your paper on DCM_estimation. could I contact you directly to discuss about it by personal email?
Hello Prindi,
about TRIAD, you can download the dcm_estimation.pdf. The implementation in Matlab is quite straightforward, you only have to type literally what is in the equations, there is not black magic.
Recently the Kalman Filter entry at wikipedia has been improved, you can start from there. I guess my code would not be so useful for you, actually I am re-writing it in a simulator, once I finish it I will free it for sure.
Let me know if you need something else.
Héctor
thank you Hector for your reply...
I'm currently reading your paper on AHRS based on UKF and TRIAD. it's my first time reading on UKF and TRIAD subject, I have implemented obert Mayhony's DCM filter. And now I am struggling with Kalman Filter in matlab. Could you tell about UKF and TRIAD or could you share the code in matlab? if you dont mind.
here is my mail: prindi.wibowo@gmail.com
thank you Hector.
Hello Prindi, Unfortunately I do not have an English version of the thesis. Nevertheless you can download the paper here:
eeexplore.ieee.org/search/freesrchabstract.jsp?tp=&arnumber=5977026&openedRefinements%3D*%26filter%3DAND%28NOT%284283010803%29%29%26searchField%3DSearch+All%26queryText%3Dukf+triad+uav
hello Hector...
Do you have ur Master Thesis in English version? I want it will be my main refrence to understand UKF.
thank you :D
@ionut
For wind estimation, I had to augment the state vector to include 3D velocity and position.
With this information (body velocity and heading derived from it), the relationship between Airspeed and Body velocity and Wind Velocity is: Airspeed^2 = [ V_body sin(heading) - V_wind sin(wind_orientation) ]^2 + [V_body cos(heading) - V_wind(wind_orientation)]^2
You can use the UKF to estimate the wind_orientation angle and V_wind, assuming that they remain constant throughout the mission (in other words, they time derivatives are zero, just the same assumption with the gyro's bias). Moreover, you can use another wind estimators published in this webpage to feed the A^2 equation, so the estimation should be with more accuracy.
@Lew
Thanks Lew xDD, well, here planes and women work well together :P