T3

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

You need to be a member of diydrones to add comments!

Join diydrones

Email me when people reply –

Replies

  • 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

      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

      • Bill 

        thanks very much .

        I got it.

        that's very helpful.

        thanks

  • 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?

    • 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.

    • 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

  • 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

  • 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

      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

      • 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

This reply was deleted.

Activity

Neville Rodrigues liked Neville Rodrigues's profile
Jun 30
More…