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

Johnson,

Thanks for your questions.

Regarding renormalization introducing drift, in practice, it does not create much drift. I have done static (motionless) tests on the UDB5 hardware with drift correction turned off and measured the residual drift from all sources (such as noise, renormalization, offset shift) to be on the order of one degree per minute. There are much larger sources of error in flight due to gyro calibration errors. See for example:

http://diydrones.com/profiles/blogs/sustained-rotations-pushing

In any case, you need to apply continual drift compensation in flight to prevent accumulation of attitude estimation error. This is done with the aid of known reference vectors.

Regarding the bottom row of the matrix, it represents the image of the earth vertical axis in the body frame. You can use the accelerometer as a reference vector for vertical if you compensate for centrifugal acceleration. There are a couple of ways of doing that, DCMDraft2 describes one way to do it, the following describes another way:

http://diydrones.com/forum/topics/an-improved-method-for-accounting...

Regarding the interpretation of the sign of the accelerometer data, depending on your preference, you can think of it as producing gravity minus acceleration, or acceleration minus gravity. I have been told by some of the members of the MatrixPilot team that the way that I think of it is opposite to the rest of the world, so I apologize if I have confused you.

Finally, here is another source of information on this subject, it is nicely organized, fun to read and easy to understand:

http://www.starlino.com/dcm_tutorial.html

Best regards,

Bill

Replies

johnson April 25, 2016 at 1:00am

Dear Bill

"Magnetometer is used for yaw compensation."

I think there is little mentioned about this approach in your DcmDraft paper

would you please give more information about how to compensate the yaw correction by Magnetometer?

Thanks
- T3
  
  William Premerlani > johnson April 26, 2016 at 3:17pm
  
  Johnson,
  
  There are several methods for using a magnetometer for gyro drift compensation. Here is the method that I like to use:
  
  1. Transform the 3D measured magnetic field vector from body frame to earth frame.
  
  2. Take the cross product of the horizontal plane projections of actual and measured earth frame magnetic field vectors. This gives you the sine of the yaw error angle. Call this the yaw correction in earth frame.
  
  3. Transform the earth frame yaw correction vector back into body frame and use it for gyro drift compensation.
  
  You may find some of the discussions on this page interesting:
  
  http://diydrones.com/page/uav-devboard
  
  Best regards,
  
  Bill
  - johnson > William Premerlani May 1, 2016 at 10:40pm
    
    Bill
    
    thanks very much .
    
    I got it.
    
    that's very helpful.
    
    thanks
johnson April 21, 2016 at 11:41pm

Dear
I have new questions
regarding the RollPitchCorrection ,we use the accelerometer to obtain a gravity reference vector
question 1:
when we use it ,is there an assumption made that the plane keep going forward in a horizontal level?
question 2:
we take the cross product Z row of estimated DCM with the gravity reference vector
and if the result is not ZERO, you call it RollPitch error
but I wonder is there also Yaw error in the DCM certainly? why do we not call it RollPitchYaw error?
is it known as RollPitch error not RollPitchYaw error ?

question 3:
respectively in the GPS setion, why we call it YawCollection? if Yaw need collect, i think the others(Roll,Pitch)also need

question 4:
in GPS section,YawCorrection
you use the X column of the DCM,that means the projection of xb of plane in the earth reference frame.
and then use the COG vector which is also treated in the earth frame.
but why only the Z component of the cross product of these two vector is seen as rotational correction?
actually I don't know much about the Yaw error(/collection) after I read this GPS section.
in my own understanding ,Yaw error means the Z Row of the estimated DCM is not aligned with the vertical gravity vector both in Plane frame's view. and at the same time as I mentioned in question 2, the Roll ,Pitch is also has errors
why these errors are seperately explained in different section. why I think 3 axis is supposed to have the errors at the same time .
all of 3 axis have errors
or
all of 3 axis do not have errors
would you please give me some explanation?
- johnson > johnson April 22, 2016 at 1:21am
  
  Very sorry to trouble you so many times.
  I have understood and let me answer some of the question myself.
  
  >question 2:
  >we take the cross product Z row of estimated DCM with the gravity reference vector
  >and if the result is not ZERO, you call it RollPitch error
  >but I wonder is there also Yaw error in the DCM certainly? why do we not call it RollPitchYaw error?
  >is it known as RollPitch error not RollPitchYaw error ?
  
  >question 3:
  >respectively in the GPS setion, why we call it YawCollection? if Yaw need collect, i think the others(Roll,Pitch)also need
  
  >question 4:
  >in GPS section,YawCorrection
  >you use the X column of the DCM,that means the projection of xb of plane in the earth reference frame.
  >and then use the COG vector which is also treated in the earth frame.
  >but why only the Z component of the cross product of these two vector is seen as rotational correction?
  
  firstly keep it in mind that the GPS COG vector in the earth fram is always located in the X-Y horizontal
  vector like [x,y,0]
  when we refer to the GPS COG, we focus the direction the plane is pointing
  if it has a error(a deviation from GPS COS) we have to correct it.
  simpliy we rotate the roll-axis,of cource we can not rotate only 1 axis due to the direction cosine matrix property
  so the pitch has to be rotated correspondedly.but the yaw-axis do not have to .
  this correction we call it YawCorrection.
  OK,because GPS COS is horizontal, so we only need to calculate the cross prodcut of XBp and COG
  which happends to be the Z component of cross product of XB and COG
  if we use the whole cross of XB and COG, as a result ,we will rotate the Yaw-axis,that is not supposed to be.
  
  OK I hope this will help other newers.
  
  Thanks.
- johnson > johnson April 22, 2016 at 12:53am
  
  additional question
  
  regarding COG and roll-axis, we only need the Z component
  
  while gravity and Z row of DCM, we nee the whole cross product
  
  may i know the reason
johnson April 21, 2016 at 11:41pm

Dear I have new questions regarding the RollPitchCorrection we use the accelerometer to obtain a reference vector ( the Z-axis in plane frame) and the assumption is made that the
johnson April 21, 2016 at 12:23am

Dear

Regarding the Eqn 26 in the paper(DcmDraft2)

I think

the acceleromter is aligned with the plane(body).so its output is the combination result of gravity and centrigual acceleration.

and we only need the gravity acceleration as our reference vector.

so the equation should be (Vector) as follows?

g_reference = Acceleration - centrigual acceleration

I confused with the Eqn26

Thanks.
- T3
  
  William Premerlani > johnson April 21, 2016 at 4:24pm
  
  Johnson,
  
  Thanks for your questions.
  
  Regarding renormalization introducing drift, in practice, it does not create much drift. I have done static (motionless) tests on the UDB5 hardware with drift correction turned off and measured the residual drift from all sources (such as noise, renormalization, offset shift) to be on the order of one degree per minute. There are much larger sources of error in flight due to gyro calibration errors. See for example:
  
  http://diydrones.com/profiles/blogs/sustained-rotations-pushing
  
  In any case, you need to apply continual drift compensation in flight to prevent accumulation of attitude estimation error. This is done with the aid of known reference vectors.
  
  Regarding the bottom row of the matrix, it represents the image of the earth vertical axis in the body frame. You can use the accelerometer as a reference vector for vertical if you compensate for centrifugal acceleration. There are a couple of ways of doing that, DCMDraft2 describes one way to do it, the following describes another way:
  
  http://diydrones.com/forum/topics/an-improved-method-for-accounting...
  
  Regarding the interpretation of the sign of the accelerometer data, depending on your preference, you can think of it as producing gravity minus acceleration, or acceleration minus gravity. I have been told by some of the members of the MatrixPilot team that the way that I think of it is opposite to the rest of the world, so I apologize if I have confused you.
  
  Finally, here is another source of information on this subject, it is nicely organized, fun to read and easy to understand:
  
  http://www.starlino.com/dcm_tutorial.html
  
  Best regards,
  
  Bill
  - johnson > William Premerlani April 21, 2016 at 9:50pm
    
    Dear I reviewed the Eqn14~17 which someone feels confused maybe. would it be better if we add some tips or comments about this setction
    1. the θ(x/y/z) in the matrix of Eqn 17 stands for the magnitude of the Vector : dt * W(x/y/z) X r(t)
    not the angel between the r(t) and r(t+dt)
    while the definite DCM use the cosin of this angle.
    
    2. I draw a draft and try to distinguish them
    and I am not sure it is correct but i think so. I hope this will help other readers

of 7

This reply was deleted.

Discussions

robust estimator of the direction cosine matrix

Replies

Activity