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 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 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" ?
-
Permalink 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.
-
Permalink Reply by Louis LeGrand on -
Greetings,
I took a look at the Mahoney paper, and I am wondering about the implications of the discussion that follows eq 8. Quote: "We assume that there is a cutoff frequency (typically around 0.1 to 1 Hz) below which the measured acceleration is a reasonable approximation of the gravitional force". Another quote: "The estimator design is based on the assumption that (the direction of the measured acceleration) is a reasonable low frequency approximation of the inertial z-axis in the body-fixed frame". These seem like a valid assumptions in a "low-dynamics" situation (i.e. low velocity). However, when the velocity is high, this seems like a problem. For example, an airplane in a steady 45 degree bank angle turn moving at 60mph requires over 17 seconds to complete one full circle (see e.g. http://www.aerospaceweb.org/question/performance/q0146.shtml). During the turn, there is a large (45 degree) discrepancy between the measured acceleration and the gravitational force, even when the accelerometer readings are averaged over 1 second (or even 10 seconds).
It would seem this could have serious implications for your plans to control a high performance airplane. Then again, I might be missing something. Any thoughts?
-
Permalink Reply by William Premerlani on
-
Louis,
Great question. Thanks for asking. I am glad to see that you read the Mahoney paper.
The Mahoney paper is just the starting point, it explains the general approach. Paul Bizard and I have improved on it, using some ideas we had, and some ideas that we have seen expressed on this web site. We will eventually write a paper explaining how everything works. We have entirely overcome the issues you raise. We have an algorithm that can be used on very high performance aircraft to do aerobatics. Paul has simulated it, I have implemented it and tested it. It works rather well.
In particular, the centrifugal effect that you refer to can be compensated. The centrifugal force can be computed from the cross product of the rotation rate vector (measured by the gyros) and the velocity vector, and subtracted out from the accelerometer readings to determine the gravity-only vector.
I have already implemented the centrifugal compensation, and tested it out. It works rather well. I tested it around a traffic circle in my car, going rather fast, tires squeeling, almost in a spin-out. Without the compensation, the algorithm had a large error. With the compensation, the error was close to zero.
Regarding some of the other assumptions that Mahoney makes, Paul and I were able to relax them. We discovered that the gyros are much more stable than we thought they would be, mostly due to the way the algorithm works. So, we were able to lower the gains of the PI feedback controllers, rejecting more of the transient errors from the GPS and accelerometers.
Anyone who has my board and the roll-pitch-yaw demo firmware could actually run it up to the full speed capability of the GPS (around 650 meters/second, I think it is) and go around a continuous circle with a centrifugal acceleration of 3 times that of gravity, for as long as they cared to go, and still have a rather accurate set of direction cosines.
Best regards,
Bill
-
Permalink Reply by Dave_fr on -
Hi there I'm new here.
I have read what you said about your method of compensating centrifugal effect and there is something that I don't quite understand. Where do you get the velocity vector from ? I understand you can determine the orientation of that vector by integrating the gyros reading but how can you get the actually velocity value. I've always asked myself how the artificial horizons work. I came to the conclusion that, as far as the pitch is concerned there wasn't much of a problem because unless the airplane faces a strong and long forward acceleration (engine surge or drag which is generally very brief), the gravity on the pitch point of view is pretty much the real gravity. Now as far as the roll is concerned, the only relation I found between the bank and what is felt inside is the G force which is equal to 1/cos(Alpha) Alpha being the bank.
However, I understand that in a turn, enough information is available (rate of turn is not null) to fully determine the velocity vector and work the system. But what about the case of a straight and levelled flight ? In other words if the airplane has been flying straight for a long time then the velocity estimated by the system can be wrong thereby messing up the compensation during the next turn.
I know that the probability of this post being helpful is very remote but it was't clear for me.
Best regards
Dave
-
Permalink Reply by William Premerlani on
-
Dave,
We get the velocity from the GPS and assume the aircraft is moving in the direction it is pointed. The centrifugal compensation is computed in the aircraft frame of reference, by taking the cross product of a velocity vector aligned with the roll axis, with the gyro rotation vector, which is measured in the aircraft frame of reference, producing the centrifugal acceleration, as seen in the aircraft frame of reference.
To be sure, there are several approximations involved. However, the drift compensation portion of the direction cosine matrix (DCM) algorithm is tolerant of transient errors, it only needs a compensation with correct average value over a time period of about a minute. Many of the errors introduced by the approximations average out if the aircraft is making a turn. If the aircraft is traveling in a straight line, then the gyro rotation vector is zero, and the compensation is zero in any case.
Best regards,
Bill Premerlani
-
Permalink Reply by Dave_fr on
-
Hi there,
First let me tell you that I'm amazed by all the work already accomplished !
About your answer I didn't realize that the gps was part of the loop. By the way, would the attitude control be affected in case of a gps failure ?
Now I have a few more questions :
-What sort of output do the gyros/accelerometers use ( digital or analog) ?
-How do you program this pic30 ship ? What platform and what hardware ?
I'd love to be able to get started with the understanding of the programming of this board !
It's very plesant to go back to school mermories with the linear alegebra and matrix ect...
Many thanks for your answer.
I'm going back to my basic pics....
Best regards,
Dave
-
Permalink Reply by William Premerlani on
-
Dave,
In the event of a GPS failure, the best you could do for attitude control would be to assume the aircraft continued to fly at the last reported speed. Compensation for centrifugal acceleration would not be perfect, but it would good enough, particularly if the turns were not too tight.
However, without GPS, the yaw drift compensation would stop. The effect would depend on when that happened. The longer the drift compensation runs, the better the removal of the gyro offsets. After a while, the residual error is very small.
By the way, I have been using GPS for return to launch for 5 years, it has never failed, not even once. Up in the air, away from buildings and trees, is a perfect place for a GPS to get a good signal. Its a sure thing. The only way for the GPS to fail is hardware error.
The gyros and accelerometers are analog.
To program the dsPIC you need a programmer/debugger, an interactive development environment (IDE), and a C compiler. For the programmer/debugger, you can get an ICD3 from Microchip, or an ICD2 from SparkFun. Sparkfun lists the ICD2 as a related item for the UAV DevBoard. Microchip also has a free IDE and C compiler.
Best regards,
Bill Premerlani
-
Permalink Reply by William Premerlani on
-
Louis,
One other thing, the Mahoney paper was focused on vertical take off, with accelerometers, gyros, and magentometers for sensors, so their assumptions are reasonable enough for their application. Their key idea and approach is to directly integrate the nonlinear differential equations governing the kinematics of rotation, and adjust for gyro drift with some low-drift "benchmark" vectors.
-
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?
-
Permalink 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"?
Top Content
Groups
-
ArduCopter User Group
477 members
-
ArduPlane User Group
299 members
-
ArduCopter (traditional …
77 members
-
ArduPilot (legacy) User …
77 members
-
Moderators
50 members
-
ROS User Group
31 members
-
Team Prometheus
11 members
© 2012 Created by Chris Anderson.
