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 -
Sean,
You might want to take a closer look at Mahony's papers. You can do all of the gyro drift compensation calculations in either quarternions or in direction cosines. You do not need to use Euler angles.
When you use direction cosines, you do the correction by taking the cross product of a vector estimated via direction cosines, and its corresponding benchmark. The result is the correction rotation vector that you feed to the PI controller.
Since Mahony uses magnetometers, I am sure that one of his papers must explain it. Also, as you point out, Louis uses a magnetometer, so I am sure he could explain it. Paul Bizard and I use GPS and accelerometers, but the approach is similar.
For GPS, I take the cross product of the column of the DCM that aligns with the direction the plane is pointing in the horizontal plane, with the GPS horizontal course over ground vector.
For the accelerometers, I take the cross product of the "down" vector computed by the accelerometers, with the vertical earth axis.
-
Permalink Reply by Jose Luis Cortes on -
I am trying to understand this attitude estimation method,
i am several days with a problem and i would like to consult.
I have implemented your DCM update in MATLAB.
omegacorr = omegacorrPAcc + omegacorrIAcc + omegacorrPMag + omegacorrIMag;
omegatotal = gyro1 + omegacorr;
theta = omegatotal;
rup = [ rmax -theta(3) theta(2)
theta(3) rmax -theta(1)
-theta(2) theta(1) rmax];
rmat = (rmat*rup);
I am making a comparison with euler integration:
i_roll = i_roll + gyro(1);
i_pitch = i_pitch + gyro(2);
i_yaw = i_yaw + gyro(3);
there are one first stuff than i can not understand
i am scaling gyro measurement (in rad/sec) * 0.00075!!!!
result seems to be correct
but this "integrated" DCM is very strange, it is only correct to North direction!!!
when i turn to south then roll value is exacly inverse values!
Any idea?
-
Permalink Reply by Jose Luis Cortes on
-
i attach 2 screenshot with this problem.
if someone are interested in matlab files i can send you - Attachments:
-
-
report1.png, 103 KB -
report2.png, 108 KB
-
-
Permalink Reply by Jose Luis Cortes on
-
i attach a new screenshot, it is with euler integration:
i_roll = i_roll + gyro(1);
i_pitch = i_pitch + gyro(2);
i_yaw = i_yaw + gyro(3);
it works ok, (with known euler problems!) - Attachments:
-
-
report3.png, 54 KB
-
-
Permalink Reply by Jose Luis Cortes on
-
Sorry, not 0.00075 for gyros scalling (i mistake) it is 0.3
gyro_for_DCM= 0.3*giro_measurement (in rad/sec)
-
Permalink Reply by Sean McDonald on -
Hi Jose,
I would be interested in looking at the matlab implementation you have. Thx Sean
-
Permalink Reply by D. Scott Kinnear on
-
I too am was just starting to do this in MatLab. If anyone has it done already I would be very grateful to be able to work with debugged files. It's tough enough to understand all the theory and learn matlab in the process.
-
Permalink Reply by UFO-MAN on -
Hi Jose, I am interested in taking a look at your Matlab files.
UFO-MAN
-
Permalink Reply by michelle hughley on
-
I need help building a transformation matrix from NED to ENU. I have to multiply roll * pitch * yaw. I've been trying to figure this out but this isn't my area of expertise. Can someone please assist me with this problem?
Thanks in advance,
Michelle
-
Permalink Reply by Tom Yochum on -
Are you sure you need a transformation from NED to ENU? If so, here's the matrix version:
C = [0, 1, 0; 1, 0, 0; 0, 0, -1]
Do you need the transformation from body frame to NED frame? That would use the roll, pitch, and yaw angles.
Check here for the Euler transformations.
-
Permalink Reply by michelle hughley on
-
Thanks. I'm still not sure how you got this answer, but thanks a lot.
-
Permalink Reply by AdrienB on
-
Hi,
Thank you for the interesting algorithm, and for all the detailed information given in the comments !
I think the compensation of the centrifugal force is very promising, and I wanted to understand a little bit better how you implemented it with the GPS velocity and to get your thoughts.
In particular, how is the GPS velocity used to compensate the centrifugal force ? My impression (is it correct ?) after very quickly looking at the code, is that the velocity is simply assumed to be along the x body axis.
Do you think there could be an algorithm to obtain a better estimate of the velocity vector ? (For example, in Mahony's paper there is an angle-of-attack model, etc...). Because the better the velocity vector is approximated, the better the centrifugal force compensation is.
For example, if the GPS velocity vector is used (as opposed to only the norm), it can be transformed to the body frame if the yaw is known (I assume the yaw can be inferred from the GPS heading). But is the GPS velocity vector reactive enough during turns, I don't know.
A problem also, is probably in case of side wind : in this case the yaw is not really the GPS heading anymore, and it is probably harder to get right (I guess magnetometers are needed here - same as when the estimator is used on a helicopter). So in case of wind, the GPS speed cannot be transformed to the body frame accurately I suppose.
Btw, have you already addressed the problem of wind in your algorithm ?
Thanks in advance for your answer !
Best,
Adrien
Social Networking
Top Content
Contests
Season Two of the Trust Time Trial (T3) Contest has now begun. The fourth round is an accuracy round for multicopters, which requires contestants to fly a cube. The deadline is April 14th.A list of all T3 contests is here.
Groups
-
Moderators
24 members
-
ArduCopter User Group
1298 members
-
Melbourne
47 members
-
ArduCopter Italian Group
87 members
-
Andropilot Users Group
183 members
Advertisement
© 2013 Created by Chris Anderson.
Powered by
