robust estimator of the direction cosine matrix
If you downloaded MatrixNav from this page before 4/29/2009, you should be aware that there is a newer version of the firmware, MatrixNavRv2, that reduces the GPS latency, and will perform much better than the first version.
I have been working with Paul Bizard on something we call the "Premerlani-Bizard robust direction cosine matrix estimator". It is based on the work of Mahony et al. The idea is to continuously update the 3X3 matrix that defines the relative orientation of the plane and ground reference frames, using GPS and 3 gyros and accelerometers. The basic idea is to use gyro information as the primary link between the two reference frames, and to use GPS and accelerometer information to compensate for gyro drift. We are working on the theory together. Paul is performing simulations. I am testing ideas in my UAV DevBoard. We have made a great deal of progress. There are demos available, and control and navigation firmware is available. The steps of the algorithm are:
1. Use the gyro information to integrate the nonlinear differential equations for the time rate of change of the direction cosines.
2. Renormalize the matrix to guarantee the orthogonality conditions for a direction cosine matrix.
3. Use speed and gyro information to adjust accelerometer information for centrifugal effects.
4. Use accelerometer information to cancel roll-pitch drift.
5. Use GPS information to cancel yaw drift.
By the way, the algorithm should work in any GPS, gyro, accelerometer nav system on a plane. Without magnetometer information, it will not work on a helicopter.
This discussion will provide progress reports from time to time. At this point we have completed all steps. Firmware and documentation for various demos and flight firmware are available on the UAV DevBoard main page.
Firmware and documentation of a roll-pitch-yaw demo program are available. There is also a first draft of an explanation of the algorithm.
If you have a UAV DevBoard, I highly recommend that you try the demo program, it is very easy to use, and runs without a GPS. During its development, I found that the gyro drift was much less than I thought it would be. After I added the drift compensation, the resulting roll-pitch peformance is nothing less than astounding.
Flight testing of "MatrixNav" is also complete. Firmware and documentation are available on the UAV DevBoard main page for stabilization and return-to-launch functions for inherently stable aircraft that are controlled by elevator and rudder. MatrixNav is implemented with a direction cosine matrix, and supercedes GentleNav. Anyone who has GentleNav should replace it with MatrixNav. Pitch stabilization is excellent under all conditions. Return to launch performance is excellent under calm conditions, and good under windy conditions. If you have the UAV DevBoard and an inherently stable plane, you will definitely want to try out MatrixNav.
Finally, AileronAssist, for the stabilization and RTL aircraft that have ailerons, is available.
What Paul and I are going to tackle next is altitude control.
Bill Premerlani
I have been working with Paul Bizard on something we call the "Premerlani-Bizard robust direction cosine matrix estimator". It is based on the work of Mahony et al. The idea is to continuously update the 3X3 matrix that defines the relative orientation of the plane and ground reference frames, using GPS and 3 gyros and accelerometers. The basic idea is to use gyro information as the primary link between the two reference frames, and to use GPS and accelerometer information to compensate for gyro drift. We are working on the theory together. Paul is performing simulations. I am testing ideas in my UAV DevBoard. We have made a great deal of progress. There are demos available, and control and navigation firmware is available. The steps of the algorithm are:
1. Use the gyro information to integrate the nonlinear differential equations for the time rate of change of the direction cosines.
2. Renormalize the matrix to guarantee the orthogonality conditions for a direction cosine matrix.
3. Use speed and gyro information to adjust accelerometer information for centrifugal effects.
4. Use accelerometer information to cancel roll-pitch drift.
5. Use GPS information to cancel yaw drift.
By the way, the algorithm should work in any GPS, gyro, accelerometer nav system on a plane. Without magnetometer information, it will not work on a helicopter.
This discussion will provide progress reports from time to time. At this point we have completed all steps. Firmware and documentation for various demos and flight firmware are available on the UAV DevBoard main page.
Firmware and documentation of a roll-pitch-yaw demo program are available. There is also a first draft of an explanation of the algorithm.
If you have a UAV DevBoard, I highly recommend that you try the demo program, it is very easy to use, and runs without a GPS. During its development, I found that the gyro drift was much less than I thought it would be. After I added the drift compensation, the resulting roll-pitch peformance is nothing less than astounding.
Flight testing of "MatrixNav" is also complete. Firmware and documentation are available on the UAV DevBoard main page for stabilization and return-to-launch functions for inherently stable aircraft that are controlled by elevator and rudder. MatrixNav is implemented with a direction cosine matrix, and supercedes GentleNav. Anyone who has GentleNav should replace it with MatrixNav. Pitch stabilization is excellent under all conditions. Return to launch performance is excellent under calm conditions, and good under windy conditions. If you have the UAV DevBoard and an inherently stable plane, you will definitely want to try out MatrixNav.
Finally, AileronAssist, for the stabilization and RTL aircraft that have ailerons, is available.
What Paul and I are going to tackle next is altitude control.
Bill Premerlani
Replies to This Discussion
-
Permalink Reply by Alvaro on -
What a great job!
Now I´m working in a similar project. Few diferences, like that I´m using ARM Cortex M3 micricontroller (STM32-H103), SparkFun 5DOF IMU plus other gyro from ST for yaw (I can´t remenber his name, LYS..). a 3 axis magnetormenter with tilt compensation, with 3 axis accelerometers inside (HMC6342) and a 5Mz GPS (LS20031). This is a expensive part list...
I´m working in PCB design, setup of simulators, etc...and studing, researching... findding and reading a lot of documentation about this problem. Attitude stimate...
I was thinking about a Sigma Point Kalman Filter making sensor fussion betwen IMU, Magnetometer and GPS. But, for me, this is like to build the Chinesse Wall. When I´ll have the PCB made and software analysis implemented, probably, there will be more light in my mind, at this moment, is a good practice to read and study what you have made, perhaps, I´ll improve my mathematical profile.
Sorry for my bad english, I´m spanish!
Saludos desde Madrid,
Álvaro Alonso
-
Permalink Reply by William Premerlani on -
There is a good paper that Paul Bizard found, that includes a discussion of maintaining the direction cosine matrix, by Robert Mahony, Sung-Han Cha, and Tarek Hamel, "A coupled estimation and control analysis for attitude stabilisation of mini aerial vehicles"
-
Permalink Reply by Jesmond Gatt on -
Hi,
Have you managed to solve your problem for the gyro and accelerometer? Could you please help me since I am doing something similar?
Thanks
Jesmond
-
Permalink Reply by William Premerlani on
-
Hi Jesmond,
Who are you directing your question to, and what problem are you referring to?
We have gotten reasonably good results with a straight-forward application of Robert Mahoney's direction-cosine matrix, and we are continuing to build on top of our foundation. Recently, we achieved several totally autonomous flights.
Paul Bizard has made excellent progress applying the extended Kalman filter instead of the DCM algorithm.
Best regard,
Bill
-
Permalink Reply by Jesmond Gatt on
-
Hi Bill,
I am currently building a quadrotor for my degree project. I need help for the equations to combine my 3 axis gyro and 3 axis accelerometer. I just need the algorithm since I wil ldo all the programing on the arm7. I tried to look in various sites but I got confused. the best one I found(I think) is this http://www.starlino.com/imu_guide.html but I still got confused. So if someone has the direct algorithms, please help me.
Regards
Jesmond
-
Permalink Reply by William Premerlani on
-
Hi Jesmond,
Well, there is the "direction cosine matrix" algorithm, which is used by both the UAV DevBoard and the ArduIMU. I have recently added a magnetometer, and there is beta version firmware running on helicopters.
Robert Mahoney wrote several in-depth mathematical papers on the subject, and Paul Bizard and I summarized them.
If you have not already done so, you might want to take a look at the UDB website.
Finally, Paul Bizard is doing work using extended Kalman filtering, he has documentation and Matlab code.
Best regards,
Bill
-
Permalink Reply by Jesmond Gatt on
-
Hi William,
Thanks for the reply I will have a look at them then I post you my reply back if it would not be a problem.
Regards
Jesmond
-
Permalink Reply by Mohamed Zahana on -
Kindly can you guide me with how to use the Sfunctions written by Paul?
I got the c codes but i don't know ho to use them in MATLAB!
Thanks a lot.
-
Permalink Reply by Mohamed Zahana on
-
Problem's solved
-
Permalink Reply by William Premerlani on
-
Hi Mohamed,
I apologize for not answering your question. I am glad that you figured it out yourself.
I see that you are interested in attitude estimate algorithms. In my opinion, the best references on the subject are Paul Savage's papers and book. His book is $200, which you can buy from him. Here is a link to his website.
Attached are a couple of his papers.
Best regards,
Bill Premerlani
- Attachments:
-
-
AIAA-4228-375.pdf, 206 KB -
AIAA-4242-557.pdf, 423 KB
-
-
Permalink Reply by William Premerlani on
-
Hi Jesmond,
By the way, I forgot to mention quaternions, they are a very popular approach. The underlying theory is the same as the direction cosine matrix (integration of the nonlinear differential equations of rotation), it is just the representation is different. If you download any of the Mahoney papers, he develops his theories for both direction cosines as well as for quaternions.
I took a look at the starlino website, it has a nice explanation of the basics. The algorithm that they use is an approximation, but it may be good enough for what you want to do, particularly if you are not going to attempt inverted flight.
The direction cosine matrix algorithm, quaternions, and extended Kalman filtering are accurate over all possible orientations of the aircraft, including inverted flight.
Best regards,
Bill
-
Permalink Reply by Jesmond Gatt on
-
Hi Bill,
The problem I am finding is how to get an angle from the gyro and accelerometer readings. It has been 10 years since I left the school bench and now I am in the first year of my degree and it is a bit hard to remember all the maths since till now we have just done an introduction course for maths. I hope we I will be a bit more prepared in the next few months. Would it be a problem is someone posts a pseudocode or an algorithm (step by step for one axis) of how it should be done.
Thanks very much
Jesmond
Social Networking
Top Content
Contests
Season Two of the Trust Time Trial (T3) Contest has now begun. The fourth round is an accuracy round for multicopters, which requires contestants to fly a cube. The deadline is April 14th.A list of all T3 contests is here.
Groups
-
Agricultural UAVs
132 members
-
ArduPlane User Group
691 members
-
ArduCopter Italian Group
87 members
-
Documentation Editors
51 members
-
Turkish Ardu Group
7 members
Advertisement
© 2013 Created by Chris Anderson.
Powered by
