Quaternion-DCM hybrid discussion on DIYDrones

[Wouter van Verre]

Hi all,

On DIYDrones I came across a new discussion about quaternion/DCM. The author of the post used a quaternion/DCM hybrid solution (I believe matrixpilot also uses quaternions in some portions of the code, right?). What got my interest is that this code (based on NASA algorithm) offers exact integration. Below is a quote from the author of the discussion:
" but we know than DCM is only true for small angular rates. But what i offer is exact solution due to exact true integration in quaternion space. I suppose it will show difference when it is exposed to high turn rates. It will be also good idea to test it SIM implementation using the code i attached before. "

I was wondering whether this code could be interesting for the UDB community, as well? Or do you think the current MatrixPilot code would not benefit from exact integration?

The discussion can be found here: www.diydrones.com/group/arducopter-evolution-team/forum/topics/suggestion-for-arducopter-v2-6
For those who are interested, the original NASA algorithm can be found attached to a comment on the first page.

Kind regards,
Wouter van Verre

Sent from my iPad

[William Premerlani]

Hi Wouter,

Thanks for pointing out the discussion about DCM and quaternions on diydrones, and for giving me a good excuse to talk on this subject.

First, anyone interested in how to achieve good accuracy from any sort of algorithm (DCM or quaternion) at sustained high rates of rotation would do well to read the discussion of some research that I did to increase attitude accuracy at very high rotation rates. During the tests, I spun a UDB on a record player turn table, and discovered several interesting things:

1. The single largest source of attitude estimate error was gyro calibration. This will cause any algorithm, including DCM and quaternion, a great deal of trouble if they do not perform inflight auto-calibration of the gyros. I am not talking about gyro offset, I am talking about gyro gain, which can easily be off by 3%. At 500 degrees/second sustained rotation rate, this produces a 15 degree/sec error in attitude estimation. In the course of my research, I figured out a way to do the inflight auto-calibration.

2. The integrator in the complementary drift compensator creates a great deal of trouble at sustained rotation rates much above 50 degrees/second, and has to be turned off. This is an issue for either DCM or quaternion.

3. DCM can be made more accurate than the usual quaternion algorithm by doing a nonlinear, rather than linear computation. If you look in the rmat.c file of the latest version of MatrixPilot, you will see that there is a term in the update computation that is proportional to the third power of the rotation rate.

4. At high rotation rates, the latency of the magnetometer must be accounted for, otherwise there can be huge attitude estimation error.

Regarding quaternion versus direction cosine matrix, the real question is not which algorithm to use, but whether or not you need to use the linear or nonlinear representation of a rotation in doing the integration step.

Both quaternion and DCM enjoy the property that it is possible to exactly combine two rotations to produce the net result. In the case of DCM, this is done by multiplying two matrices.

For the integration step, in both quaternion and DCM, the update is performed by combining two rotations, in which one of the rotations represents the incremental rotation in one integration time step. In both cases, there are two ways to represent the incremental rotation. One way is an exact method that expresses the rotation in terms of a rotation axis and the trigonometric sine and cosine of the rotation angle. The other method is a linearization of the non-linear, exact, trigonometric expressions in which the cosine is approximated as being equal to 1, and the sine is approximated by the magnitude of the rotation.

In both quaternion and DCM, the linear form of the update will lead to small (much smaller than gyro calibration error) errors at high rotation rates. These errors can be eliminated by using the exact expression for the update rotation, or greatly reduced by using a Taylor's expansion of the trig functions.

In the latest version of MatrixPilot, sin(theta) is approximated as theta-(theta**3)/6, which produces much better accuracy than the linear quaternion update method. (Actually, there is another subtlety involved, which I explain in my report, so the correction factor is not 1/6.)

If you are going to use quaternions at high rotation rates, you should not use the commonly published versions of the update algorithms, which are linear, but rather you should use either the exact non-linear form, or use several terms in the Taylor's expansion.

For more information on the relative merits of matrices and quaternions, I suggest this book.

The bottom line is that I do not see any clear advantages of DCM or quaternion per se, but you have to address several issues in order to achieve good performance at high rotation rates.

Best regards,

Bill Premerlani

[Raynor]

Bill,

I am interested in the math you used in the rmat.c rupdate method.

As of SVN rev 2851 the method described in the fast rotations report appears to be used (1 + (theta**2)/3).
This appears to match what was posted here if cos(theata) is assumed to be 1?
Where did the 1/6 vs 1/3 go?

Also, theta seems to be computed from the sum of the squares of vector theta.
Should it be the magnitude, or does the square root not matter since this is a gain factor?

Your insights are greatly appreciated,
-Raynor

[William Premerlani]

Raynor,

Your question arrived just in time for me to answer it, before I go offline for 3 weeks.

The short answer is that the math is subtle, the implementation is correct and has been verified by testing on a record turntable at 78 RPM.

The adjustment is in fact (1 + (theta**2)/3), where theta is the magnitude of the rotation, in radians.

Since the adjustment is based on the square of theta, it cancels with the square root you would need to get theta, and you wind up with theta**2 equal to the sum of the squares of the thetas for each axis.

Regarding your question about why the adjustment is 1/3 instead of 1/6, I will give you a hint:

cos(theta) ~ 1 - (theta**2)/2

sin(theta) ~ theta*( 1 - (theta**2)/6)

1/2-1/6 = 1/3  

For more information, read the discussion referred to in the posting that was referred to below:

"First, anyone interested in how to achieve good accuracy from any sort of algorithm (DCM or quaternion) at sustained high rates of rotation would do well to read the discussion of some research that I did to increase attitude accuracy at very high rotation rates. During the tests, I spun a UDB on a record player turn table, and discovered several interesting things"

But, before I close, I will note that I have been digging into this subject some more, and that there are other corrections that can be made, such as an adjustment for "coning", such as when the aircraft is spinning, and the spin axis is rotating around another axis. I attach a couple of papers on that subject.

Best regards,

Bill

--
--
---
You received this message because you are subscribed to the Google Groups "uavdevboard" group.
To unsubscribe from this group and stop receiving emails from it, send an email to uavdevboard...@googlegroups.com.
For more options, visit https://groups.google.com/groups/opt_out.