T3

### robust estimator of the direction cosine matrix

#### Replies

• Bill,

In your paper you talk about how using gyros with accelerometers helps overcome acceleration issues such as false 'pitch during launch'.

The current ArduIMU DCM implementation shows considerable pitch changes during acceleration when tested in a vehicle. ie when you drive off from a standing start the DCM Eulers show up to 15deg pitch up and vice versa during braking

Is this an indication that something is not quite right in the ArduIMU DCM implementation?

Thanks
Andrew
• Hi Bill,

I am currently trying to solve the following issue but do not seem to make much progress, and was hoping you could maybe shed some light on the subject (will be looking into it again tomorrow).

I am implementing the DCM algorithm for use in a land based vehicle and it has proven to work quite well (thanks!). The last few yards is now to get it to work correctly when the vehicle is reversing. This is a bit tricky since GPS speed is always positive. I jumped that hurdle by simply integrating the accelerometer value for a few seconds after GPS speed is zero, getting a direction indication easily enough (I know this is not 100% failsafe, but it is acceptable for my application).

In the forward direction the centrifugal effect is compensated for nicely when turning, but in the reverse direction the corrected pitch is actually tracking the raw roll angle (instead of being compensated) and vice-versa. Difficult to explain without a graph, but I hope you get the idea. Is there something obvious that I am missing for the reverse direction when it comes to the centrifugal correction.

gplane[0]=gplane[0]- omegaSOG( omegaAccum[2] , (unsigned int) sog_gps.BB ) ;
gplane[2]=gplane[2]+ omegaSOG( omegaAccum[0] , (unsigned int) sog_gps.BB ) ;

Regards,
Elardus
• Dear Bill, I have a question about the integration period of PI Controller of DCM.
It seems to be a long term period which covers whole flight time.
It doesn't like other general PID Controller which limitted only in a very short time gap.
• Hello all! I come to you humbly to discuss using magnetometers in the DCM algorithm. I believe that my questions will reflect my lack of understanding here! Currently I am working with the propeller chip and have successfully demonstrated pitch/roll control using the Sparkfun 5dof IMU and the DIYdrones gyro daughter board. So far so good. I also have a GPS available but out of stubbornness I would like to use the HMC5843 3 axis magnetometer as a proof of concept platform.

Ok, so here it goes. Am I going about this correctly? My though process is as follows:

1) To use the three axis magnetometer I need to essentially rotate the magnetic readings from the inertial body coordinate system back to the earth reference frame. (Pitch and roll only.)

2) From there I can compute my heading.

3) Then I can essentially follow the steps from the DCM paper regarding yaw drift cancellation using a GPS signal.

Is this reasonable or is there a much easier way? I feel like there should be....
• As i understood,the RMarix outputs measure values of the roll, pitch and yaw which corrected by GPS and acc.
I want to add a "fly by wire" function into DCM.
Following is my PID algorithm.
//From RMarix get corrected values of roll,pitch and yaw.
Meas_roll=(the roll angle from Rmarix);
Meas_pitch=(the pitch angle from Rmarix);
Meas_yaw=(the yaw angle from Rmarix);

//setpoint value is tilt angle of stick of RC transimitter .
Setpoint_roll=(the tilt angle of roll_stick);
Setpoint_pitch=(the tilt angle of pitch_stick);
Setpoint_yaw=(the tilt angle of yaw_stick);
//Error=Measure value-tilt angle of stick.
Error_roll=Meas_roll-Setpoint_roll;
Error_pitch=Meas_pitch-Setpoint_pitch;
Error_yaw=Meas_yaw-Setpoint_pitch;

// Integration of Error_roll
Error_roll_sum=Error_roll_sum+Error_roll;
Error_pitch_sum=Error_pitch_sum+Error_pitch;
Error_yaw_sum=Error_yaw_sum+Error_yaw;

//
Roll_control=Error_roll*P+Error_roll_sum*I;
Pitch_control=Error_pitch*P+Error_pitch_sum*I;
Yaw_control=Error_yaw*P+Error_yaw_sum*I;
// send above values into mixer() function to control servo.
Mixer(Roll_control,Pitch_control,Yaw_control);

I don't know whether this PID algorithm can work with DCM.
Somebody could give me a better suggestion about "fly by wire"?
In MatrixNav it seems to be only two kinds of control modes--Automatic and Manual.
How to intergate the stick signal into DCM?

.
• can anyone explain me how equation 14 became a matrix form in equation 17 ? (refer to DCMDraft2.pdf)

i don't understand how "r(t) + r(t) x d0(t)" there's an addition and a cross product became matrix multiplication by a scalar in equation 17..

---
herry w.
• Dear Bill,

Thank you for the excellent work that you, Paul and Louis has done for us drone diy'ers in deriving and formulating the complementary DCM attitude estimator using gyros, accelerometers and GPS data.

I am happily porting over your DCM code to a dsPIC hardware platform, and reconciling the differences in axes definition and gyro/accel sign directions amongst your paper, the version8a code and my code is causing a certain blankness in the mind :)

If it is not too much trouble, would you be able to assist in providing the DCM output from your board and code for a few IMU attitudes:
a. roll=0 pitch=45 yaw=0
b. roll=0 pitch=45 yaw=90
c. roll=45 pitch=45 yaw=90?

The data would be of great help for me to remap and confirm the output that I should be getting from my board and code.

I am using a 30F6014A board with IDG500 gyros, MMA7260 accels, MicroMag3 magnetometer, and floating point code.

Thank you,
Koide
• AHHH HELP!!! OK, so I bought the ardupilot main board and the daughter boards to make a 6-dof monster. Currently, I am using the propeller chip and integer operations to get things up and going. Suffice to say that I have the bulk of the math in good working order!! The DCM estimation algorithm with orthogonality and normilization works as I would expect with small errors creeping in over time. Now comes the fun part, I cannot for the life of me figure out how to implement the accelerometer corrections. It seems to me that adding them to the bottom row of the DCM (the Z row) and then only using the X and Y rows for the orthogonality/normalization correction completely negates any information the accelerometers are providing.

So to summarize, I do not understand how adding the acceleration vector to the DCM Zrow and then multiplying it by the new gyro signals (which does not change the X or Y rows of the resulting matrix to reflect the Zrow of the original DCM) and then replacing the Zrow with the cross product of the X and Y rows retains any information regarding the acceleration. English is failing me here. Please let me know if this unclear... Thanks!
• Hi Bill,
Last few days I started "really "looking into DCM (yes I know, I am behind the class :) ). I don't have a DevBoard, nor ArduIMU so I can't practically test it (I'll get one of the two) , but have more of a math question:

If dir. cosine matrix is as follows:

0.707107 0.707107 0
-0.707107 0.707107 0
0 0 1

how do you know if plane banked (roll) 45 degrees or rotated (yaw) for 45 degrees? (because matrix looks the same)
Do you make a decision based on sensors - ie. detected gyro sensor roll so I know I banked rather then noticed GPS heading change (for yaw). or is this taken care of in SW somewhere...?

Since I am looking at this from pure math perspective, matrix was set-up in 3D Cartesian coordinate such that +z axis is up, +y is through nose of the plane, and +x is out of right wing
• Soo, is there any consensus on the applicability of Sparkfun's 5 dof IMU as it applies to the DCM estimation routine? Are the gyros (IDG500's) excessively noisy/drifty?(I don't think 'drifty' is a word, but whatever, it sounds cool.) I was hoping to save some money and pair the 5 dof unit with one of DIYDrones LISY300 daughter boards for the 6th dof... Comments?