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

### Activity

Mike Whitney liked Mike Whitney's profile
Jul 19
Jun 26
Abdulatif Tlaib liked Ryan's profile
Jun 24
RT @SmallpixelCar: First time at @wsraceways. After tuning @HokuyoUsa lidar signal and camera sensor on @NVIDIAEmbedded, my car was able to…
Jun 20
RT @breadcentric: The handy bit about racing with self-driving cars is that one can type while the car is racing. Report from #AWSDeepRacer…
Jun 18
RT @chr1sa: Our Bay Area @DIYRobocars meetup is now at 2,700 members. Next in-person events (in Oakland) are training day on July 17 and…
Jun 18
Videos from the ICRA autonomous racing workshop are now available: https://linklab-uva.github.io/icra-autonomous-racing/
Jun 10
RT @SmallpixelCar: Prepared race track for Warm Spring Raceways @wsraceways and looking forward to test my new car at RAMS RC @ramsaicar fa…
Jun 6
RT @f1tenth: Trying out some nasty blocking maneuvers 🏎️🤖 #f1tenth #autonomousracing https://t.co/nMTstsaogM
Jun 5
RT @araffin2: I will talk this Saturday from 18:00 to 19:00 Paris time for the @diyrobocars community about learning to race in hours using…
May 27