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 -
refer to Louis, (1-based indexing)
phi = atan2(-DCM23, DCM33)
theta = atan2(DCM13, sqrt(1-(DCM13)^2))
psi=atan2( -DCM12, DCM11)
and the firmware on ArduIMU_pack,
theta=asin(DCM[3,1])
---
the angle of phi & psi is varied from 0 to 180 then -180 to 0, that is good to be used in labview cube, the cube could rotate 360o
problem is on theta which is varied from -90 to 90, it doesnt represent a 360o rotation in labview's cube, it screw the rotation visualization..
any thought ?
---
another question about Gyro_Gain and Ki Kp value, how to find it's best value ?
-
Permalink Reply by William Premerlani on -
Hi Herry,
The simplest thing to do when you want to display a labview cube is to work with direction cosines, not with Euler angles. The three rows of the direction cosine matrix are the three axis of the plane, as seen in the earth frame of reference. So, all you have to do is plot each row as a vector, and you will have exactly what you want for a display.
As far as the Euler angles are concerned, phi and psi have a range of plus or minus 180 degrees, theta is only supposed to be between plus and minus 90 degrees. That covers all possible orientations.
For more information, see Wikipedia, for example.
Also, if you want to see how Euler angles are computed in MatrixPilot, take a look at an explanation that I wrote.
Best regards,
Bill
-
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
- ‹ Previous
- 1
- …
- 14
- 15
- 16
- Next ›
© 2010 Created by Chris Anderson
