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
-
T3Permalink 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"
-
T3Permalink Reply by William Premerlani on -
This is an update. Implementation of the "Premerlani-Bizard robust direction cosine matrix estimator" is complete. The firmware that I used to develop and test the algorithm, as well as demo documentation, are now available. Performance is solid, to say the least. Those of you who have purchased a UAV development board are definitely going to want to try out this demo to gauge the possibilities for the board and the firmware. Here is what is planned next:
1. Documentation of the theory.
2. Simulation results.
3. Revision of the GentleNav firmware to take full use of direction cosines for performance that is more accurate, responsive, and stable than the present revision.
4. New firmware that will stabilize an aerobatic plane using ailerons, rudder, and elevator. The functions (stabilization and return to launch) will be the same as those in the GentleNav firmware, except they will work for a high performance plane that need not be inherently stable. The author plans to use this firmware in the spring to fly a plane that he crashed 5 times last summer, hopefully without ever crashing again.
Bill Premerlani
-
Permalink Reply by Simon Kerr on
-
Awesome work Bill and Paul! Is there any possibility or plans to get the "Premerlani-Bizard robust direction cosine matrix estimator" working on an Arduino chip as opposed to the dsPIC? It would be great if people could "upgrade" from IR thermopiles to a full IMU while keeping the same board and just attaching the sensors and uploading new firmware. That way people would remain within their Arduino "comfort zone".
Would the ATMega 168 be powerful enough / have enough memory / have enough IO ports, or would this need an ATMega 328, or even the Illuminato?
-
T3Permalink Reply by William Premerlani on -
Simon,
I think it would be great idea to port the cosine estimator to an Arduino chip. I would be happy to work with anyone who is familiar with the Arduino, and who wants to do it. Though, they might have to wait a while, because I have quite a bit on my plate right now.
I think that the ATmega 168 might be powerful enough. Right now, the algorithm takes up about 2% of the CPU power of a dsPIC running with a 16MHz clock. It does not take up much memory, only about 50 integers. The "only" IO ports that it needs are 6 analog inputs for the gyros and accelerometers.
Another option would be for me to develop an IMU board that would inteface digitally with the Arduino. The board would have 3 gyros, 3 accelerometers, and a dsPIC which would compute the direction cosines. It would need some GPS information, which would be furnished by the Arduino.
That way you could have the best of both worlds. The algorithm would not need to be ported, it would be "encapsulated", and you could still work within your Arduino "comfort zone".
What do you think? Should I get started on a "direction-cosine IMU board"?
-
Permalink Reply by OlivierD. on
-
William, this is impressive work. An IMU board that could interface with ArduPilot? That's like a dream.
This way, there could be IMU based autopilots for low budgets and the UAVDev board for higher budget.
Regardless, I am saving up for the UAV Dev board but I wouldn't mind seeing my ArduPilot work with IMU in a near future. It would be fun to experiment with both.
Congrats to you and Paul for the outstanding work so far. Truly inspiring stuff. I can't wait for actual flight reports. I am checking the stock situation for the UAVDev Board on sparkfun everyday, I am almost ready!
Regards,
-
T3Permalink Reply by William Premerlani on -
OlivierD,
Thank you very much.
I will give flight progress reports here as soon as I do some flying. I am afraid that I will have the firmware ready way before any of the places that I fly are ready. I live in upstate New York, and all of my fields will be bogs as soon as the rest of the snow melts. It has been a long, hard winter, and I am more than ready to go flying, but it may be a while.
best regards,
Bill
-
-
Same here, I am in Montreal, it's mud time for the next little while but spring is around the corner at last! The consolation : More time to build. I am building a 12 foot span glider with a power pod. For now, ArduPilot with FMA sensors but I have been watching your developments carefuly, regardless, I will buy a UAV Dev board because I think IMU is superior to IR. Too many lakes and trees where I fly so IR is not reliable, unless I hit an open field. I will purchase my UAV Dev by mid to late April, shortly after tax time! Until then, it's torture. Good think I have the mighty little ArduPilot as well to play with otherwise I would have to seek therapy! Chris and Jordi have done amazing work with this as well.
kind regards,
Olivier D.
-
Permalink Reply by automatik on
-
Nice !
I was thinking of making "IMU shield" for the Arduino but it seems you are more then half way there.
Any time estimate for "Documentation of the theory" ?
-
T3Permalink Reply by William Premerlani on -
automatik,
Documentation of the theory should be ready in about a month. In the meantime, there is a good paper on the subject.
-
-
Hi William, sorry for the intromission...
I will try to port the algorithm to the ARM Cortex M3 as it is right now. I just made the PCB and will start ADC testing and scheduling this weekend. I am trying to made a IMU board that will transmit serial data at a high speed to any microcontrolled autopilot. I was thinking about Sigma Point Kalman Filter to integrate gyros, accelerometers, magnetometer and GPS...
As you can read below in this post, I have a magnetometer too, could it be integrated in the "Premerlani-Bizard robust direction cosine matrix estimator"?
-
T3Permalink Reply by William Premerlani on -
Alvaro,
A magnetometer could be integrated. In fact, the paper that describes the theoretical basis of our algorithm was written specifically for using a magnetometer for yaw adjustment.
BIll
Top Discussions
Groups
-
UgCS
272 members
-
Telemetry over cellular …
563 members
-
Pixhawk2.1
51 members
-
Agricultural UAVs
1065 members
-
ArduCopter User Group
3157 members
© 2019 Created by Chris Anderson.
Powered by
