T3

### robust estimator of the direction cosine matrix

#### Replies

• I am interested in getting the DCM Lite to run on the parallax propeller. I have a 5 DoF board, is that sufficient to run the DCM lite code?? As I understand the the DCM lite code only estimates pitch and roll, however dont you still need the yaw gyro to make accurate estimations of pitch and roll.

FYI here is a fixed point object for the propeller
http://obex.parallax.com/objects/501/
• Hi all!

I have "Programmed" the DCM estimation algorithm in Matlab and seem to understand how it is working. The next step I am taking is translating it to the Propeller chip. Using floating point operations and the full renormalization scheme, I am getting a clock speed of ~25Hz!!! AHHHH!!! So I have a few questions...

First, is it at all reasonable to assume that the DCM matrix is symmetric? (Cuts out half the math operations.) From what I can see with the Matlab file this is not entirely accurate, but has anyone else played with this?

Second option, is anyone here versed enough with the propeller chip to propose a faster floating point matrix multiplication scheme?
• Hi Bill,

I finally read your paper "DCM first draft" and I have a question.
In Eqn 25, in order to estimate the Acentrifugal, you make the assumption that, in the plane reference frame, the velocity vector is [GPS Speed, 0, 0]. Well l I have a problem undestanding how it can handle a continuous turn in windy conditions. Are you relying on the fact that over a long period of time it averages out ? For exemple let's say we have a plane TAS = 40 km/h and a 30 km/h wind. During the upwind part of the turn the GPS speed is low then Acentrigufal is computed as very low therefore the DevBoard sees the Gravity as being almost parallel to the Zframe vector and that regardless of the actual bank, it will then correct the gyros accordingly and the bank will increase. Obviously I know that the opposite will occure during the downwind leg and it should bring the bank back the correct level.
So I suppose that it all comes down to the characteristics of the PI correcter ,the period has to be long enough right ?
I'm a fan of your work and just want to fooly ;-) understand your theory and how you can work without an airspeed sensor or estimation of the TAS which for a full sized pilot like me sounds amazing.

Best regards,

Dave
Domain im Kundenauftrag registriert
Domain registriert bei united-domains.de
• Bill, do you know if anyone has made significant progress in porting your method over to the Arduino? I'm getting ready to do so myself but certainly wouldnt mind if someone else had already done some/all of the heavy lifting.

Thanks
• Great project.

I have a bit of a different take and project I'm working on - not flying, but on the ground/water.
the idea is to record accel/gyro/mag sensor readings then run these through a program and reconstruct the path that the sensor took.

I'm completely new to this 6DOF/DCM stuff. So, I expect to ask some silly questions. Sorry in advance.

Does anyone have a matlab/simulink version of the DCM implemented here ? Or maybe a PC version of the DCM code ?

FYI - it's for a good cause. This is being used for a study of autistic children, and animal behavioral research (very non-profit - as in out of my pocket / donated time kind of non-profit). Think of putting a simple sensor on, that only records the sensors, for hours. Then we analyze the movements later. We don't need to do this real time on the sensor board, as there is nothing to control.

Whatever analysis I get done I'll be glad to post back. I would love to have a matlab/simulink simulation setup, so that I could play with different A/D configurations / different filter configurations / different update rates and on and on.

Thanks
Scott K.
• hello all,
I am new to this forum and have a question. Does any one know a method to compensate the gyro drift using particle filters in the manner kalman does it...using both the gyro readings and calculated angles from acclerometers. I need to compensate the drift using particle filter only...! Any help would be very much appreciated. Also if you could refer to some reading material on the topic then it would be great of you... I have tried reading different resources but maths involved is way too high for me... :(

Awaiting ur swift reply...!
• I just had quick read through ur paper. How does it compensate for sidewind? Gps give out course over ground, but if there is wind the tail needs to be paralell to the total speed component.

And how is it possible to extract winddirection when you know airspeed, groundspeed and course over ground, ?

Do u use the acceloremeter to take out the speed?

Kim
• @William: You are most generous. I have now downloaded the draft, and will be digesting it this evening (and probably several evening hence!). I'm sure I'll have questions ;-)

Thank you.
• That is some impressive work done by the Premerlani-Bizard team and of course the rest of the people taking their work forward to new places.
Can anyone share a code in a generic form? Floating point C or Matlab (Sean, Jose…)
I think it will help many people struggling with Mahony et al and the fabulous fixed point coding Bill has done.

ManorA
• I'm working on a floating point PC port of DCM right now. Identifying the extra/missing constants related to switching between the 1.15 and 2.14 formats required figuring out some of the underlying math, which was enjoyable, but took longer than I'd like to admit.

Here's one example. rmat contains 3 orthonormal vectors; one per spatial axis. These rotate to represent the plane's orientation vs an external reference frame (usually Earth). See the great explanations earlier in this thread.

It is critical that these vectors remain orthogonal (perpendicular) to each other, and also, that they remain normal (length=1), where length is sqrt(x^2+y^2+z^2). Without infinite precision math, eventually they will become non-normal, and this will quickly cause a numerical saturation or overflow in the fixed point types. It's a problem in floating point as well; other parts of the implementation use simplifications which depend on length=1.

To renormalize we just need to divide each vector by its scalar length. x^2+y^2+z^2 is easy to compute but how about the division and sqrt()? Bill uses a simple but surprisingly accurate approximation.

Using Newton's Method we can approximate 1/sqrt(S) iteratively with

Xn+1 = (Xn / 2) * (3 - S*(Xn)^2))

Where Xn is the current estimate and Xn+1 is the next estimate, and S is the function input.

What makes this great for a nearly-normal vector is that the initial estimate (X0) is 1.

So, X1 = 1 / 2 * (3 - S*(1)^2)

Simplifying,

X1 = (3/2) - (S / 2)

That's simple enough. 1 subtraction and 1 shift for a fixed point type.

How well does 1 iteration work?

octave:21> format long
octave:22> 1/sqrt(0.9)
ans = 1.05409255338946
octave:23> (3/2)-(0.9/2)
ans = 1.05000000000000
octave:24> 1/sqrt(1.05)
ans = 0.975900072948533
octave:25> (3/2)-(1.05/2)
ans = 0.975000000000000
octave:26> (3/2)-(1.001/2)
ans = 0.999500000000000
octave:27> 1/sqrt(1.001)
ans = 0.999500374687773

This is running after every sensor update and it does a good job of renormalization, very quickly with very little code.
This reply was deleted.