trying to implement DCM...

[Mike Palmer]

Hi, all

I have read William Premerlani's theory paper several times and a lot of it is beginning to soak in, but I must be missing somthing in my implementation.

I am trying to keep an accurate view of earth-relative pitch and roll angles on an embedded device with 3 axis of rate gyros and 3 axis of accelerometers.

My approach is apparently flawed, and I'd appreciate any and all feedback. The approach is to maintain only the bottom row of R matrix as follows:

(1) initialize the bottom row of R matrix floating point values (eventually I'll use integer but for now, I'd be happy to get the floating point working even though it's much CPU cycles) by performing necessary trig operations on an initial known, stable value for pitch (theta) and roll (phi), obtained from accelerometer with nothing but 1g acting on the accelerometers.

(2) thereafter, perform the following at a fixed, quick time interval (I've tried both 20 ms and 40 ms intervals).
(2A) measure the three gyro ADC values.
(2B) construct the assymetric matrix on page 15 of the theory paper by multiplying these readings (normalized to radians per second) by the time interval (ie, 0.020 or 0.040).
(2C) update the bottom row of the R matrix by multiplying its current row values by this assymetric matrix.
(2D) calculate current pitch from -asin ( R matrix row 3, col 1)
(2E) calculate current roll by knowing 2D value and using asin of R matrix row 3, column 2)


So... I'm only maintaining the bottom row of R matrix.

Somewhere, my logic is flawed! I have a fixture on which I can rotate around earth's yaw axis and also adjust and fix target's roll and pitch angles relative to earth. I am constantly feeding the values back for real-time display at PC.

I fix the pitch at 45 degrees and roll at 0 degrees, for example. Before turning the fixture, I see values at the PC for pitch, roll, and also the 3 values for the bottom row of the R matrix.

Before moving the fixture, I see a nice 45 degree pitch, 0 degree roll, and -0.707, 0, 0.707 displayed at the PC. These drift a bit after 20-30 seconds, but that's ok, for now, since I'm doing nothing to correct for drift yet.

But when I spin the fixture, the values immediately go very wrong.

What flaw(s) are in my approach?

Any and all comments are greatly appreciated.

[Brian Wolfe]

Mike,
To be a valid DCM matrix you can't just initialize the bottom row. You need to initialize and maintain the entire matrix. What are you putting in the other matrix elements, 0's or 1's or something else?? To initialize the matrix to a starting angle, you can use the same approach as creating the delta matrix, but if the angles are significant you shouldn't use the small angle approximations, but rather the full blown trig implementation. You only have to do that once at he beginning, so time shouldn't be an issue. I was looking to do this myself at some point but it's down the list a ways. Let me know if you need the full trig representation of the matrix.

I should also add that currently I boost the gain of the drift compensation loop by a factor of ten at power up for a few seconds to help force the DCM to align with the accelerometers, then I back off on the gain.

Brian

[Mike Palmer]

Brian - thanks for the feedback, much appreciated.

But I do not understand why, for my application, I cannot get by with just using and maintaining the bottom row only. I can calculate pitch and roll completely from this bottom row by using the following, can't I?

first, perform -asin (first element) to get theta
then once I know theta, I can get its cosine, divide the second element by that, and do asin( result ) for roll.

To update the bottom row completely at each interval, all that seems to be needed is the complete delta matrix as well as the current bottom row.

Something's not clicking to me.

[Brian Wolfe]

Hey Mike,
I see. Your not concerned with yaw at all and are essentially just rotating a vector that represent down (from which you are deriving pitch and yaw). Can you post a snippet of code that shows the order of the multiplies? Also, are you sure the vector is normalized after each pass? Conceptually I don't see a problem yet but I'm still noodling.

Brian

[William Premerlani]

Mike,

hmmmmm....

You raise an interesting question that has been in the back of my mind for some time...

I sometimes run my roll-pitch-yaw demo without benefit of yaw drift compensation. It works beautifully. Roll and pitch lock in, while there is a very slow yaw drift. The point is, roll and pitch work ok without having yaw alignment. So, it would seem that there should be a way to maintain the bottom row only...

Off the top of my head, it would seem to me that what you are trying to do should work. I think that you should be able to ignore yaw, and maintain just the bottom row of the R matrix doing what you said. Multiply the bottom row vector by the rotation matrix to do the update. I assume that you did that in the right order? (vector times matrix, not matrix times vector?). Then you would renormalize the bottom row for magnitude, and perform the drift cancelation with the accelerometer. (You said that you are not doing that yet, but I do not think that it would matter.)

In any case, up until now, I have not dug into the theory of this question, but now that you have asked, I will look into it when I get a chance to see whether the theory supports what you are trying to do.

Best regards,
Bill Premerlani

[William Premerlani]

Mike,

The theory supports what you are trying to do. It should work.

I have helped a few RC pilots port DCM to their own custom boards, and the most common mistakes that I see are confusion over assigning the 3 gyros to their correct axes, and getting the signs right. A common error is confusion over which is the pitch axis and which is the roll axis.

In the situation that you describe, if you get the axis assignments or signs wrong, the values will go immediately wrong when you spin. I suspect that is the problem.

Best regards,
Bill

[Mike Palmer]

Thanks Brian and Bill for your quick responses. I appreciate very much.

I'm still having no luck getting my device to correctly track a fixed pitch and roll angle when I spin in on a test fixture. It does seem to track fairly well when I change just pitch and/or roll without spinning.

I hesitate to add my code, since I would imagine all viewers are very busy with your own projects, but if you do have time to look at (I cut the applicable portions and attached), much appreciated again. I "brute forced" and tried all possible sign combinations for the delta thetas, but to no avail thus far. Note that the attached is cut and pasted test code which is horribly expensive in CPU cycles, I know!

I am considering calibrating the gyro gain, instead of using the hardcoded numbers based on spec and ckt analysis of op-amp gain that currently determine the "magic numbers" in translating analog inputs to radians.sec.

Thanks much,
-Mike

[Brian Wolfe]

Hey Mike,
I'm at work so don't have time to look at your code real close right now, but a quick glance and it seems more complex than it needs to be. It's probably close, but I'll have to spend more time looking. In the meantime I type up a general approach that I think is fairly readable. See if this is basically what you did. Note I don't have the drift compensation included here. Also Note I'm calling the third row in the DCM that your concerned about
Down_Vector for lack of a better name.
Down_Vector[0] = X;
Down_Vector[1] = Y;
Down_Vector[2] = Z;

Convention is X forward, Y out the right side and Z down. Sign convention for the rates is right hand rule (positive rate for clockwise motion looking in positive direction on each axis)


/*******************************************************************************************************/
/* Update the Body to NED DCM */
/* A standard DCM when expressed in terms of body euler angles is as follows: */
/* Pitch Angle = P, Yaw Angle = Y and Roll Angle = R */
/* |Cos(P)Cos(Y) Sin(R)Sin(P)Cos(Y)-Cos[R]Sin[Y] Cos(R)Sin(P)Cos[Y]+Sin[R]Sin[Y]| */
/* |Cos(P)Sin(Y) Sin(R)Sin(P)Sin(Y)+Cos[R]Cos[Y] Cos(R)Sin(P)Sin[Y]-Sin[R]Cos[Y]| */
/* | -Sin(P) Sin(R)Cos[P] Cos(R)Cos[P] | */
/* */
/* If we assume the angles are very small, which is valid if we sample at a high */
/* Enough frequency, then we can use the approximations: */
/* Sin(Small Angle) = Small Angle */
/* Cos(Small Angle) = 1 */
/* Small Angle x Small Angle = Very Small Number = 0 */
/* Small Angle x Small Angle x Small Angle = Very Very Small Number = 0 */
/* */
/* This Simplifies our Matrix to: */
/* | 1 -Yaw Pitch | */
/* | Yaw 1 -Roll | */
/* | -Pitch Roll 1 | */
/* */
/* We can assemble this matrix very quickly with the delta angles computed from the */
/* gyro sensor data. */
/* We can then multiply this matrix by our current Body to Ned DCM to transform it */
/* by our new delta angle DCM */
/* We now have a new Body to NED DCM */
/* Finally, this DCM is re-normalized */
/* */
/* 34uS */
/***************************************************************************************/

static float Down_Vector[3] = {0, 0, 1};
float Temp_Down_Vector[3];
float Delta_Body_DCM[3][3];


/* Compute Delta Angles over last sample period Using Gyro Data(scaled to Rad/S) */
Delta_Roll_Rad = (Body_Roll_Rad_S * Sample_Period);
Delta_Pitch_Rad = (Body_Pitch_Rad_S * Sample_Period);
Delta_Yaw_Rad = (Body_Yaw_Rad_S * Sample_Period);


/* Construct the off-diagonal elements of the update matrix */
Delta_Body_DCM[0][0] = 1.0;
Delta_Body_DCM[0][1] = -Delta_Yaw_Rad;
Delta_Body_DCM[0][2] = Delta_Pitch_Rad;

Delta_Body_DCM[1][0] = Delta_Yaw_Rad;
Delta_Body_DCM[1][1] = 1.0;
Delta_Body_DCM[1][2] = -Delta_Roll_Rad;

Delta_Body_DCM[2][0] = -Delta_Pitch_Rad;
Delta_Body_DCM[2][1] = Delta_Roll_Rad;
Delta_Body_DCM[2][2] = 1.0;


/* Now Multiply The Down_Vector by the Delta Body DCM to get the new Down_Vector */
Temp_Down_Vector[0] = Down_Vector[0]*Delta_Body_DCM[0][0] +
Down_Vector[1]*Delta_Body_DCM[1][0] +
Down_Vector[2]*Delta_Body_DCM[2][0];

Temp_Down_Vector[1] = Down_Vector[0]*Delta_Body_DCM[0][1] +
Down_Vector[1]*Delta_Body_DCM[1][1] +
Down_Vector[2]*Delta_Body_DCM[2][1];

Temp_Down_Vector[2] = Down_Vector[0]*Delta_Body_DCM[0][2] +
Down_Vector[1]*Delta_Body_DCM[1][2] +
Down_Vector[2]*Delta_Body_DCM[2][2];

/* Normalize The Down Vector (very important) */
Magnitude = sqrt((Temp_Down_Vector[0]*Temp_Down_Vector[0]) +
(Temp_Down_Vector[1]*Temp_Down_Vector[1]) +
(Temp_Down_Vector[2]*Temp_Down_Vector[2]));

Down_Vector[0] = Temp_Down_Vector[0]/Magnitude;
Down_Vector[1] = Temp_Down_Vector[1]/Magnitude;
Down_Vector[2] = Temp_Down_Vector[2]/Magnitude;


/* Now you can extract your Roll and Pitch data from the Down_Vector terms */
/* Down_Vector[0] = cos(angle between NED Z axis and body X axis) */
/* Down_Vector[1] = cos(angle between NED Z axis and body Y axis) */
/* Down_Vector[2] = cos(angle between NED Z axis and body Z axis) */

[William Premerlani]

Mike,

I replied to your last post, but my reply seemed to have disappeared. In the meantime, I took a quick look at your code.

In one of your previous posts, you mentioned that you were going to update only the last row of the matrix. I am rather excited about that idea. I think of it as "DCM lite". It produces 2/3 of the results (pitch and roll) with only 1/3 of the work of the full DCM algorithm.

When I looked at your code, though, I see that you are implementing the full algorithm. Did you decide to give up on "DCM lite"?

You mentioned that pitch and roll worked ok if you did not apply yaw. That would suggest that you have the gains correct.

What happens if you yaw 360 degrees? Do the values come back to their approximate starting point?

I will take a closer look at your code when I have some time. In the meanwhile, it looks like Brian Wolfe is going to help you, too. Thank you very much Brian!

Best regards,
Bill Premerlani

[Mike Palmer]

Brian and Bill -

Thanks so much for spending the time to try to help me out. I already owe you both a pizza, on me, should you ever happen thru Elkhart, Indiana!

Brian - yes, I'm following most of your algorithm almost exactly. The only part I do not follow and do not understand is your final step... where you're using (in the comments), I think, acos( Down Vector[] ) to get pitch and roll in earth frame. I was using -asin( Down Vector[0]) since Down Vector[0] = -sin(theta) from page 10 of Bill's paper, I thought.

Bill - I have not abandoned DCM lite, although I had been trying a full version to see if the fact that I was not orthagonalizing / normalizing was causing the problem. I have actually spent a good deal of time working both ways (see "updated attachment, which includes my (flawed) lite and full codes.)

If I yaw 360 degrees, yes, I'm pretty much back at the start point, so you're probably on to something, although I thought I tried all possible sines and also swapped axis.

Also... I am using a CAN data bus to connect my device to a PC program, and I am sending the following values to the PC for graphic display as I yaw the target 360 degrees in roughly 7-8 seconds. The .zip file is a bitmap showing the values:

(1) gyro raw ADC X,Y,Z (order of text at left = order of graphs at right)
(2) pitch and roll calculated according to just acceleros and also DCM gyros.
(3) the bottom row values, each multiplied by 1000.

Ideally, I believe the bottom row values should remain pretty much constant during the entire movement.

Thanks again,
Mike

[William Premerlani]

Mike,

The fact that a yaw of 360 degrees brings you back to your starting point is a great clue because:

1. It proves that you have the right gyro gains.
2. It greatly narrows down the suspects. There are only a few ways for that to happen. My main suspect still is switching the roll and pitch gyros, and or signs, though you said you already tried every combination.

When the board is pitched 45 degrees, and then you yaw the board around the earth z axis, the signal from the three gyros should be:

yaw gyro : absolute value is .707 of yaw
roll gyro: absolute value is .707 of yaw
pitch gryo: 0

Then, the calculations will result in no change in the bottom row. So, take a look at the bottom row, look to see what the relationship between yaw and roll signals must be for there to be no change when pitch is 45 degrees.

For some reason, your assumed pitch axis is not lining up with your math. In other words, you have set up an intial condition for the matrix that does not agree with the actual orientation of the board. This is proven by the fact that you get back to the starting point with a 360 degree rotation.

Regarding "DCM-lite", if you compute only the bottom row, you do not have to make any orthogonality adjustment, you only have to normalize the magnitude back to 1.

So, I suggest that you should go back and try to get "DCM-lite" working first, it will be easier to debug, I think.

Here is a way to check your axes: pitch your board 45 degrees, the way you said, and then do a yaw in the earth frame of reference. You should see signals on the yaw and roll gyros, but nothing on the pitch gyro. If you see something on the pitch gyro, and nothing on the roll gyro, then you have the roll and pitch gyros mixed up.

Best regards,
Bill

[William Premerlani]

Mike,
It just occurred to me that my code may have confused you...the axis conventions I use in my firmware are not the "standard" conventions that I used in the documentation that I wrote concerning DCM. I wrote my firmware before Paul Bizard set me straight on what the accepted conventions are for axes and signs. Here are the standard conventions:

First regarding vector measurements (like acceleration, velocity, gravity):

X axis points forward, toward the nose of the plane.
Y axis points along the right wing.
Z axis points straight down. (go figure)

The gyro that measures a rotation around the X axis is the roll gyro, and is called the X axis gyro. Positive roll around the X axis lifts the left wing up.

The gyro that measures a rotation around the Y axis is the pitch gyro, or the Y gyro. Positive pitch lifts the nose up.

The gyro that measures a rotation around the Z axis is the yaw gyro, or the Z gyro. Positive yaw (which is viewed "on your back" looking up at the plane) turns the plane to the right.

In your experiment, a pitch is a rotation around the Y axis. In terms of DCM, it changes only the first and last elements of the bottom row. After that, when you yaw, you should see only X and Z gyro outputs, nothing on the Y.

Best regards,
Bill Premerlani

[Brian Wolfe]

Mike,
I took a quick look at your plots. I'm still a little confused about units and variable names, but it almost looks like you've got x and y rates swapped. If you have you IMU pitched up 45 degrees then yaw it around this orientation you should get roll and yaw rates from your sensors of equal and opposite values and zero pitch rate. It looks like your getting 0 roll rate and equal and opposite pitch and yaw rates.

to clarify, A roll rate occurs around the X axis. A pitch rate around the Y axis and a Yaw rate around the Z axis. The sign of the rates should follow the right hand rule.

Maybe I'm reading the graph wrong. It also looks like there is a bias in the rate data but maybe that's subtracted out down stream.

Brian