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 herryw on -
Thx Sir, I got it.
what about Gyro_Gain and Ki Kp value, how to find it's best value ?
-
Permalink Reply by William Premerlani on -
Hi Herry,
The actual values of Ki and Kp depend on the units that you are working in, and the gains of some of the other calculations that you are doing, such as the drift detection. But perhaps this will help:
Ki and Kp form a feedback loop through the DCM algorithm. The values are not critical, but I find that setting them for a time constant of around 10 seconds, with approximately critically damped behavior, is ok. So, here are two ways you can set them:
1. Analyze the feeback loop through your implementation of DCM, and adjust the Ki and Kp for about a 10 second time constant, critically damped.
2. Set the gains using the same technique that I used, as follows:
A. I used my roll-pitch-yaw demo as a test bed.
B. I intialized the matrix with the wrong values.
C. I observed the response of the demo after power up.
D. I adjusted Ki and Kp to get about a 10 second time constant, and critically damped response.
Best regards,
Bill
-
Permalink Reply by William Premerlani on
-
Herry,
Good question, why not D term in the drift compensation, since the PID controller is commonly used?
The short answer is I always try to keep things as simple as possible, and no simpler than that. Now a little bit longer answer.
You definitely want the I term for the drift compensation feedback controller. The integral vector will eventually settle out to the offsets of the gyros, and it will compensate for gyro offset with zero error in the direction cosines. Without the integral term, there would be steady state errors. The I term allows the plane to fly perfectly level under DCM control, even with small gyro errors, such as might be caused by temperature effects, for example.
You definitely also want the P term for the drift compensation feedback controller, to make it stable and well damped. Without the P term, the I term by itself would tend to cause ringing.
We do not need or want a D term in the gyro drift controller, because the drift of the gyros changes very slowly. So, if we applied a D term, all that we would be doing is injecting noise from the reference signals (GPS and accelerometers), whose signals change orders of magnitude more rapidly than the actual drift of the gyros. With a D term, the performance of the DCM algorithm would be degraded.
Interestingly enough, what I found to be the best approach for controlling the roll, pitch, and yaw of the plane is PD (proportional plus derivative)
In the case of the plane, the D term is useful in countering the effects of external disturbances, such as turbulence and wind gusts, which would otherwise tend to rotate the plane very quickly. It is easy to detect such disturbances with the D term. And you also want the P term, because without it, there would be an offset in the response. So, for example, in the pitch control, its the P term that keeps the plane level, and the D term that resists sudden changes in pitch.
I tried full PID controls as well for yaw, pitch, roll, and found that it had some problems, and did not really improve overall performance. The main problem was that it tended to produce a delayed, unexpected responses in stabilized mode and waypoint mode. What happened was that the primary commands from either the pilot or the waypoint logic might make a change in the attitude and/or altitude of the plane, and then the integrators in a full PID control would gradually respond, creating an undesireable transient.
So, in the end, I decided to use PI controllers for gyro drift compensation, and PD controllers in pitch, roll, and yaw control.
Nowhere did I use a full PID controller.
Best regards,
Bill
-
Permalink Reply by herryw on
-
very clear answer, thx.
i just finally made my own board work with your DCM theorem, after 4 months -_-a really happy though =)
how long did u take for understanding the mahony's and implement this DCM, Sir ? and what's your education background anyway, if u dont mind
-
Permalink Reply by William Premerlani on
-
Hi Herry,
Regarding Robert Mahoney's papers, I was not aware of them when I first started work on the DCM algorithm, about 4 years ago. Mahoney and I were working in parallel, he published first. I was delighted to see his full mathematical explanation.
My particular contribution was a simple way to assure orthonormality of the direction cosine matrix elements. Manhoney prefers to use quaternions, in which the normalization process is obvious. I prefer to use direction cosines, because they are directly applicable to stabilization, control, and navigation.
Around the time the UAV DevBoard was introduced by SparkFun, I met Paul Bizard online, and we worked out the rest of the details of the algorithm. Paul also discovered Mahoney's work, and pointed it out to me.
It took about 4 months from the time Paul and I started until we had something that would do stablized flying and return to launch. We proceeded in stages:
1. Roll-pitch demo
2. Roll-pitch yaw demo
3. Stabilized flight with rudder and elevator
4. Return to launch with rudder and elevator
5. Porting to ailerons and elevator.
After that, we added new features about one a month, until a few months ago when the size of the team expanded, and now the team is developing new features at a rapid rate.
I am recently retired from GE's research labs. I have a PhD in electrical engineering. My interests are evenly divided among mathematics, physics, engineering, electronics, flying model airplanes, and computer science.
I am one of the coauthors, with Rumbaugh, Blaha, Eddy, and Lorensen of "UML" (unified modeling language) and a couple of books on the subject of object-oriented modeling and design.
Best regards,
Bill
-
Permalink Reply by Doug Gray on -
Bill (or anyone who can assist),
I have recently purchased a SparkFun 9dof IMU board and am using the DCM firmware code available for this in my EFIS system on board my Amateur built Aircraft. The aircraft is a couple of months from flying.
I have re-implimented the DCM software so runs in the EFIS system and I receive the raw sensor data over the serial link. During the development phase this is a more desirabe solution than the fully embedded solution.
On the queston of the DCM performance I am very impressed - I intended to develop a Kalman filter solution but so far the DCM solution seems to be more than adequate.
My questions -
How to extact slip angle from the DCMMatrix?
The instrument usedto display this in an aircraft cockpit is the simple slip ball - a fluid damped ball in a semicircular tube. When the aircraft is on the ground the ball is displaced if the aircraft is other than horizontal or more importantly in the air when aircraft is 'out of balance' and there is a lateral force that some pilots can feel in the seat of their pants.
The normal response to this is to use the rudder to correct the balance.
Can you give me a lead to extracting this from the DCM matrix.
Q2 - I need to display the direction 'to the nearest horizon' - how can I extract this from the DCMatrix?
If the horizon is off the screen - chevrons appear on the EFIS pointing to the horizon - the purpose being to guide the pilot in the recovery - the drill is to 'roll to the nearest horizon and recover from the dive/climb'.
If possible I would appreciate some help with these two questions.
Thanks,
Doug
(hopefully I havn't hijacked the discussion)
-
Permalink Reply by evren yortucboylu on -
hi everyone,
first of all thanks for your excellent share.
i implemented (tried at least) dcm algorithm in C#.
connected my 6 dof IMU to PC via serial port and read raw data.
and tried a wide range of PI gains but i never achieved a good result.
is it because of gains?
one more thing is i confused with accelerometer signs. should down to earth be positive or negative?
thank you all.
best regards.
evren.
-
Permalink Reply by William Premerlani on
-
Evren,
Well, there are a lot of simple ways that implementations of DCM can go wrong. You have to have a firm grasp of vectors to get it working.
The most common mistakes are sign errors and/or figuring out the right-handed X, Y, Z coordinate systems, and getting the sensor scalings.
The aviation convention is that Z down is positive.
Conventions for accelerometers vary. Some measure postive for gravity minus acceleration, others measure acceleration minus gravity.
Best regards,
Bill
-
Permalink Reply by Jack Chen on
-
Dear Bill:
The altitude holding algorithm of MatrixNavRv2 make me very confused. MatrixNavRv2 can do a accurate altitude holding. But I don't know the MarixNav how to know current altitude? The DCM algorithm seems not provide a way to do a altitude measure.
Best Regards
Jack Chen
-
Permalink Reply by geir on
-
Hi! I had this under control. But then I stopped thinking about it for a while and no I dont understand any more :( Could any one help my understand way you take the cross product betwene Z in the earth frame of reference and the acc vector to find the error in the body frame of reffrence??
-
Permalink Reply by Dave_fr on -
You can think of Z as the "down" vector (expressed in the plane frame reference). What DCM does : From a kno n initial attitude (flat) it uses the gyros to determine the attitude change from this initial attitude. If the gyro (and no linearization assumption made ect...) you wouldn't need to correct for error (drift). In DCM the object used describe the to caracterize attitude is a DCM matrix. You could well use other objects like quaternions or eulesr angle rodrigues parameters ect... (each of them have their pros and cons).
So, if no acc, you compute the DCM every time step from the gyros but the error will start building up. This is when measurement need to be made with the acc and be compared to the estimate (DCM). For roll and Pitch, you don't need the whole DCM matrix, you can just take the third line (if I'm not mistaken) wich represent the "down vector".
This down vector can be also measure with the acc.
Now to answer your question you probalbly thing that the easiest way of comparing these to vector is to substract but by taking the cross product you will obtain a vector that represents some sort of rotation vector between to two "down" vectors. The coordinate of the cross vector can then be reinjected as gyro offset in an effort to bring the "down" vector from the DCM to the same direction as the "down" vector that is observed by the acc.
© 2010 Created by Chris Anderson.
