### DCM matrix implementation problem

Hello all, I'm Leonardo from argentina. I'm implementing dcm algorithm on an lpc1769 and can't make it work.

I test my gyro, obtain raw data, calculate standar deviation on each axis and note that l3g4200 is a litle noise gyro.

I integrate Wt over my timestep, 20msec, and obtain rotating angle for each axis. I note a drift of 1 degree per minute. I check gyro gains and seems to be very close to datasheet.

Finally a porting dcm matrix to my uC. I don't make it work. rigth now I want to implement just gyro on my DCM matrix. well, when I do that I notice that, on still position, dcm is drift too fast. i.e. it go from 1 0 0 to 0 1 0 on first row, then to -1 0 0 in 30 second... On the same time I see that angle calculated by integration is less than 1 degree...

then I make a new test. I put 0 to Y and Z gyro and work only with X gyro. when i log dcm matrix I see that is not drifting to fast, but when a rotate my gyro 4 values, of 9, on dcm matrix change a lot with litle rotation angle. and if a go back to still position dcm is not close to identity....

becouse I log angle integration and chek that the angle calculated is close to rotate angle I think thta gyro offset and gain is correct.

Now I'm asking myself if what I expect is ok,l mean, working with one axis gyro i have to see only 4 values change on my dcm matrix while y rotate de imu. and I should see that a row on dcm have to change from, i.e., 1 0 0 to -1 0 0 when I rotate 180 degrees. is that ok?

I will debug my code with the help of matlab. I'm thinking on log data with my gyro and put it onto matlab implementation and see where the error is, cheking partial calculation.

Can anyone tell me another way to verify where the error is?

thk and best regards

and sorry for my english!!!!!

#### Replies

• I'm a new question:

If i rotate the IMU at a rate of [1 2 3] deg/sec for 10 second, do I expect to obtain 10deg of pitch, 20deg of roll and 30deg for yaw?

I know about euler angles singularities, but I rotate 30deg max on yaw...

Is this equation valid to obtain euler angles < 90°?

Pitch = -asin(dcmEst(3,1));

Roll = atan2(dcmEst(2,1),dcmEst(1,1));

Yaw = atan2(dcmEst(3,2),dcmEst(3,3));

Best regards!

• T3

Leonardo,

Just to make sure you understand the basics.

The direction cosine matrix represents attitude, not rotation rate. So, there is not a mapping from gyro rates to the matrix.

Some of your replies suggest to me that you thought some of the matrix entries would be trigonometric functions of the rotation rate. If so, then that is the issue.

When the IMU is rotated from its initial position, and then stopped, the direction cosine matrix will not look anything like the identity matrix.

Best regards,

Bill

• Hello Leonardo,

Sounds like what you are doing is correct, it just wont work with noisy mems gyros. They will drift slightly with temperature changes too. You also need to use accelerometers to do drift correction on your gyro values. Then if your system is moving you will need to do centripetal corrections when you are accelerating in a circular path. I can be very complicated. Fortunately the hard work has all ready been done by Paul Blizard and William Premerlani. A good place to start is here. Good luck in your endeavors.