trying to implement DCM...

Hi, allI 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.

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

RT @araffin2: 🏎️ After hours of video editing, I'm happy to share a best of my Twitch videos on learning to race with RL. 🏎️ Each part is…

Replies

Mike Palmer July 6, 2009 at 12:46pm

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
DcmTestCode.c
T3

William Premerlani July 2, 2009 at 4:44pm

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
T3

William Premerlani July 2, 2009 at 4:03pm

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
Mike Palmer July 2, 2009 at 10:59am

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

Brian Wolfe July 2, 2009 at 10:23am

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

This reply was deleted.

Discussions

trying to implement DCM...

Replies

Activity