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 -
Hi Andrew,
I can only guess, since I have not looked at the ArduIMU DCM implementation lately. There have been hundreds of flights with MatrixPilot running on the UAV DevBoard in which stabilized mode is used routinely during takeoff, and we have not noticed any pitching due to takeoff acceleration.
Possibly the gains on the gyro drift compensation in the ArduIMU are set a bit on the high side. Uncompensated acceleration shows up as apparent gyro drift, and the PI feedback controllers will respond to it. The amount of response is proportional to the gains. So, in order to use the gyros to overcome false "pitch during launch" you have to use low gains. However, in order to recover from gyro saturation, you cannot set the gains too low. So, setting the gains is a compromise between rejecting forward acceleration and recovering quickly from gyro saturation during rotations that exceed the range of the gyros. It may be that Jordi has opted for fast recovery from gyro saturation, at the expense of some response to forward acceleration.
By the way, it was my impression that Jordi was computing forward acceleration from the GPS information, and compensating using that information, but I am not sure about that, you should check with him. If he uses that technique, he could further reduce the response to acceleration. Perhaps he is doing that, and when you were doing your tests, you either were not using a GPS, or it did not have lock.
In any case, I suggest that you contact Jordi, and see what he has to say.
Best regards,
Bill
-
Permalink Reply by William Premerlani on
-
Andrew,
One more thing, just to put the problem in perspective: the pitch error would be a lot greater than 15 degrees if you tried to use the accelerometers directly to compute pitch during a launch.
Best regards,
Bill
-
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
-
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.
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.
