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,
I realize that part of my last comment might be confusing. A clearer way to say it is the following:
In my firmware, rmat describes the orientation between these two reference frames:
Board frame of reference: X along left wing, Y forward, Z down.
Earth frame of reference: X west, Y north, Z down.
When the plane is sitting level on the ground facing north, rmat is the identity matrix.
-
Permalink Reply by William Premerlani on
-
Sean,
Just couple more thoughts, and then I promise to be quiet until I hear from you:
1. Its best to debug your code in parts. Try to get the update and normalization working first, without the drift compensation. The simplest way to turn off the drift compensation if you already have the code written is to set the loop feedback gains to zero. Even without drift compensation, the removal of gyro offset by recording the gyro values at power up is good enough to get the drift down to a few degrees per second.
2. I use the dsPIC's built in hardware to multiply 16 bit integers to produce a 32 bit result. If you are using the compiler to do this instead by defining the result to be 32 bits, you must first cast the 16 bit operands to 32 bit values. Otherwise the compiler will generate code to produce a 16 bit result, and then sign extend it.
3. You have to get the scaling factors for the gyros and accelerometers approximately right, to within about 5%. What I did for the gyros was to set the roll-pitch-yaw demo in my swivel chair and rotate it a few times to check the gyro scale factor. The first time, I was off by a factor of 2.
-
Permalink Reply by Sean McDonald on -
Thanks Bill for all the info.I will need to spend more time on refining my port of the code and I think this info should clear up some things.I had removed the drift compensation code to start debugging it, but as I only started the port on the weekend I have some ways to go. This forum is excellent and your dcm code should work for me once I get it ported correctly. Thanks. I will let you know if and when I get the code to work correctly. Thanks.
-
-
Permalink Reply by Louis LeGrand on
-
Hi Sean,
3D geometry is a bit confusing, I think. There are lots of ways to interpret the DCM, but here is one way. The DCM is the result of 3 rotations, roll (phi), pitch (theta) and yaw (psi), in that order. These individual rotations have their own DCM representation, and can be combined via matrix multiplication as follows:
Rroll = [1 0 0 ]
[0 cos(phi) -sin(phi) ]
[0 sin(phi) cos(phi) ]
Rpitch = [cos(theta) 0 sin(theta) ]
[ 0 1 0 ]
[-sin(theta) 0 cos(theta) ]
Ryaw = [cos(psi) -sin(psi) 0]
[sin(psi) cos(psi) 0]
[ 0 0 1]
To get the full DCM, first roll, then pitch, then yaw
DCM:=multiply(multiply(Rroll, Rpitch),Ryaw);
DCM =[cos(theta) cos(psi) , -cos(theta) sin(psi) , sin(theta)]
[sin(phi) sin(theta) cos(psi) + cos(phi) sin(psi) ,
-sin(phi) sin(theta) sin(psi) + cos(phi) cos(psi) ,
-sin(phi) cos(theta)]
[-cos(phi) sin(theta) cos(psi) + sin(phi) sin(psi) ,
cos(phi) sin(theta) sin(psi) + sin(phi) cos(psi) ,
cos(phi) cos(theta)]
In Bill's code (file:rmat.c, function: rupdate) he uses the gyros to build a delta rotation matrix. He then multiplies the current DCM with this delta rotation matrix to get the updated DCM (which still needs to be re-orthoganalized).
deltaDCM = [ 1 -delta_psi, delta_theta ]
[ delta_psi, 1, -delta_phi ]
[ -delta_theta, delta_phi, 1 ]
Notice that this delta DCM is exactly what you would get by substituting small angle approximations into the DCM above (i.e. cos(delta_angle) = 1 and sin(delta_angle) = delta_angle) and sin^2(deta_angle) = 0)
Anyway, to answer your original question, you can extract the roll, pitch and yaw from the DCM (using 1-based indexing):
phi = atan2(-DCM23, DCM33)
theta = atan2(DCM13, sqrt(1-(DCM13)^2))
psi=atan2( -DCM12, DCM11)
or using the Bill's array indexing
phi = atan2( -rmat[5], rmat[8] )
theta = atan2( rmat[2], sqrt(1-rmat[2]*rmat[2] )
psi=atan2( -rmat[1], rmat[0] )
Which agrees with your formula.
However, I would be careful using these angles, as they will jump from -pi to pi. In contrast, the DCM will always change smoothly.
Hope this helps,
Louis -
-
Permalink Reply by Widyawardana Adiprawita on -
According to Louis,
The formula for roll, pitch and yaw are :
phi = atan2( -rmat[5], rmat[8] )
theta = atan2( rmat[2], sqrt(1-rmat[2]*rmat[2] )
psi = atan2( -rmat[1], rmat[0] )
But why Bill use this (in the rollPitchYaw demo) :
accum.WW = __builtin_mulss( rmat[6] , 4000 ) ;
PDC1 = 3000 + accum._.W1 ;
accum.WW = __builtin_mulss( rmat[7] , 4000 ) ;
PDC2 = 3000 + accum._.W1 ;
accum.WW = __builtin_mulss( rmat[4] , 4000 ) ;
PDC3 = 3000 + accum._.W1 ;
regards
-doni- -
-
Permalink Reply by William Premerlani on
-
@doni
Doni asked this question:
"According to Louis,
The formula for roll, pitch and yaw are :
phi = atan2( -rmat[5], rmat[8] )
theta = atan2( rmat[2], sqrt(1-rmat[2]*rmat[2] )
psi = atan2( -rmat[1], rmat[0] )
But why Bill use this (in the rollPitchYaw demo) :
accum.WW = __builtin_mulss( rmat[6] , 4000 ) ;
PDC1 = 3000 + accum._.W1 ;
accum.WW = __builtin_mulss( rmat[7] , 4000 ) ;
PDC2 = 3000 + accum._.W1 ;
accum.WW = __builtin_mulss( rmat[4] , 4000 ) ;
PDC3 = 3000 + accum._.W1 ;
regards
-doni-"
The answer to doni's question is that in my roll-pitch-yaw demo, I am directly displaying three of the nine elements of the direction cosine matrix, I am not displaying phi, theta, or psi. Louis showed the nonlinear equations of how phi, theta, and psi are related to some of the elements of the matrix. The matrix elements are superior to phi, theta, and psi for both demonstration and control. Therefore I am displaying them instead of phi, theta, and psi. The matrix elements are related to phi, theta, and psi, but they are not the same thing.
rmat[6] is the cosine of the angle between the aircraft X axis and the earth Z axis. It is equal to zero when the aircraft X axis is level with the earth XY plane.
rmat[7] is the cosine of the angle between the aircraft Y axis and the earth Z axis. It is equal to zero when the aircraft Y axis is level with the earth XY plane.
rmat[4] is the cosine of the angle between the aircraft Y axis and the earth Y axis. It is equal to one when the aircraft is Y axis is aligned with the earth Y axis. -
-
Permalink Reply by Widyawardana Adiprawita on
-
Dear Bill,
Thank You for Your response, it explains a lot
regards
-doni- -
-
Permalink Reply by simonl on -
I wish I could understand all this - I know I need to to even have a chance of building my own auto-pilot - but, alas, the math is way over my head! I'm hoping you paper will shed some light (please add lots of diagrams/pictures - I'm a visual sorta guy). I'd really like to port this to a Parallax Propeller :-)
-
Permalink Reply by UFO-MAN on -
Simon, the ADIS16405 looks like a good device. But.... have you checked the price? Take a look here: http://search.digikey.com/scripts/DkSearch/dksus.dll?Detail&nam...
Digikey lists it for USD 707,- which I think is very expensive.
Datasheet here by the way: http://www.analog.com/static/imported-files/data_sheets/ADIS16405.pdf
UFO-MAN
-
Permalink Reply by UFO-MAN on
-
Correction: the post above regarding the ADIS16405 should be addressed to Jay Kickliter, not to Simon.
UFO-MAN
-
Permalink Reply by Sean McDonald on
-
Thanks Louis,
I have the problem of the jumping from -pi to +pi
I will think this through as I continue my debugging of the code.
Thx
Sean
-
Permalink Reply by Sean McDonald on
-
Bill/Louis,
The info on the matrix elements is great. I now have it responding to my inputs once I adjusted the gyro gain etc. Now I need to work on the p and i gains for pitch & roll. Without compensation for pitch roll by accel, the euler angles representation by phi the and psi drifts as expected.
If the elements are used for demo and control without conversion to quarterions or Euler angles, how would I be able to correct a magnetometer for pitch and roll? I see Louis has mag being used so I must read the paper again ... Hopefully it sinks in and becomes more evident to me. Thx all info to date. Seano
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
-
ArduCopter Italian Group
87 members
-
Andropilot Users Group
183 members
-
Melbourne
47 members
-
ArduPlane User Group
692 members
-
Japan ArduCopter Group
51 members
Advertisement
© 2013 Created by Chris Anderson.
Powered by
