# 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 follow this order:

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!!!!!

Views: 1046

### Replies to This Discussion

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.

Greg, thk for your coment.

I had read William 4 o 5 times already. I read starlino's paper too. I understand part of it. I have some doubt about implementation.

I test gyro + accel too and what I observe is that the last row in dcm stay close to 0 0 1. but first and second row drift too fast. I'm thinking on add magnetometer too, but I'm fighting with calibration.

I read that l3g4200 is a noisy gyro, but what I'm worry about is that angle integration calculation is ok and on the same time dcm matrix change to much (I'm not sure if I express myself in the correct way). I test 3 sigma (standar desviation) threshold on gyro data. In this case dcm is stable on still position, but the problem of change dcm's values too fast when I rotate the imu is still there.

so I have doubt about my code (starlino's code).

by the way, where do I find implementation code from William work? I'm looking for something simple, just gyro and accel data.... I'm looking for some GUI for a Pc to look parameters of dcm implementation...

I hope someone can help me.

thk and best regard!!!

I make some matlab script to test my code:

clear;
imu_interval=0.01975;
dcmEst=[1 0 0; 0 1 0; 0 0 1];
gyro=[-0.000476962508 -0.00100231264 -0.000903233362];
g=[1 -gyro(3) gyro(2); gyro(3) 1 -gyro(1); -gyro(2) gyro(1) 1];
dcmEst=dcmEst*g;
error=-dot(dcmEst(1,:),dcmEst(2,:))*0.5;
x_est=[0 0 0];
y_est=[0 0 0];
x_est = dcmEst(2, :) * error;
y_est = dcmEst(1, :) * error;
dcmEst(1,:) = dcmEst(1,:) + x_est;
dcmEst(2,:) = dcmEst(2,:) + y_est;
dcmEst(3,:) = cross(dcmEst(1,:), dcmEst(2,:));
dcmEst(1,:)=dcmEst(1,:)/norm(dcmEst(1,:));
dcmEst(2,:)=dcmEst(2,:)/norm(dcmEst(2,:));
dcmEst(3,:)=dcmEst(3,:)/norm(dcmEst(3,:));
gyro
dcmEst

gyro=[-0.00204776251 -0.00414881203 0.00357279973];
g=[1 -gyro(3) gyro(2); gyro(3) 1 -gyro(1); -gyro(2) gyro(1) 1];
dcmEst=dcmEst*g;
error=-dot(dcmEst(1,:),dcmEst(2,:))*0.5;
x_est=[0 0 0];
y_est=[0 0 0];
x_est = dcmEst(2, :) * error;
y_est = dcmEst(1, :) * error;
dcmEst(1,:) = dcmEst(1,:) + x_est;
dcmEst(2,:) = dcmEst(2,:) + y_est;
dcmEst(3,:) = cross(dcmEst(1,:), dcmEst(2,:));
dcmEst(1,:)=dcmEst(1,:)/norm(dcmEst(1,:));
dcmEst(2,:)=dcmEst(2,:)/norm(dcmEst(2,:));
dcmEst(3,:)=dcmEst(3,:)/norm(dcmEst(3,:));
gyro
dcmEst

gyro=[0.0533423312 0.399783999 0.0112587996];
g=[1 -gyro(3) gyro(2); gyro(3) 1 -gyro(1); -gyro(2) gyro(1) 1];
dcmEst=dcmEst*g;
error=-dot(dcmEst(1,:),dcmEst(2,:))*0.5;
x_est=[0 0 0];
y_est=[0 0 0];
x_est = dcmEst(2, :) * error;
y_est = dcmEst(1, :) * error;
dcmEst(1,:) = dcmEst(1,:) + x_est;
dcmEst(2,:) = dcmEst(2,:) + y_est;
dcmEst(3,:) = cross(dcmEst(1,:), dcmEst(2,:));
dcmEst(1,:)=dcmEst(1,:)/norm(dcmEst(1,:));
dcmEst(2,:)=dcmEst(2,:)/norm(dcmEst(2,:));
dcmEst(3,:)=dcmEst(3,:)/norm(dcmEst(3,:));
gyro
dcmEst

gyro=[0.0533423312 1.1 0.012587996];
g=[1 -gyro(3) gyro(2); gyro(3) 1 -gyro(1); -gyro(2) gyro(1) 1];
dcmEst=dcmEst*g;
error=-dot(dcmEst(1,:),dcmEst(2,:))*0.5;
x_est=[0 0 0];
y_est=[0 0 0];
x_est = dcmEst(2, :) * error;
y_est = dcmEst(1, :) * error;
dcmEst(1,:) = dcmEst(1,:) + x_est;
dcmEst(2,:) = dcmEst(2,:) + y_est;
dcmEst(3,:) = cross(dcmEst(1,:), dcmEst(2,:));
dcmEst(1,:)=dcmEst(1,:)/norm(dcmEst(1,:));
dcmEst(2,:)=dcmEst(2,:)/norm(dcmEst(2,:));
dcmEst(3,:)=dcmEst(3,:)/norm(dcmEst(3,:));
gyro
dcmEst

As you can see I put 4 DCM loop, first with low values from my gyro (still position), next with 0.4 deg/sec and 1.1 deg/sec on Y axis.

This is my matlab outputs:

>> DCM_01

gyro =

-0.0005   -0.0010   -0.0009

dcmEst =

1.0000    0.0009   -0.0010
-0.0009    1.0000    0.0005
0.0010   -0.0005    1.0000

gyro =

-0.0020   -0.0041    0.0036

dcmEst =

1.0000   -0.0027   -0.0051
0.0027    1.0000    0.0025
0.0051   -0.0025    1.0000

gyro =

0.0533    0.3998    0.0113

dcmEst =

0.9306   -0.0035    0.3661
0.0234    0.9987   -0.0456
-0.3655    0.0510    0.9294

gyro =

0.0533    1.1000    0.0126

dcmEst =

0.3567    0.0203    0.9340
0.0996    0.9943   -0.0370
-0.9297    0.1062    0.3527

>>

As you can see first two (or three) dcm matrix is close to identity and it is what I expected.

But when I rotate my gyro (slowly) and put this value on dcm calculation I obtain somthing that I don't expect. If I rotate 1.1 deg/sec dcm matrix is far away from identity... Is that right? Am I make wrong math???

Thk and best regards!

Its been a will since I studied DCM, but the identity matrix sparked my memory. Could be that your dcm is not re-normalized. From what I remember the renormalization keeps the DCM square(orthogonal).  As for a code example, try this. Its arduino C code. The core of it is not my work, but Bills theory implemented by Jordi Munoz and others along the way.  Hope this helps.

dcmEst(1,:)=dcmEst(1,:)/norm(dcmEst(1,:));
dcmEst(2,:)=dcmEst(2,:)/norm(dcmEst(2,:));
dcmEst(3,:)=dcmEst(3,:)/norm(dcmEst(3,:));

This code normalize my DCM. Take a look at -0.9297    0.1062    0.3527 (last row on last DCM):

(-0.9297)*(-0.9297) + (0.1062)*(0.1062) + (0.3527)*(0.3527) = 1.00001782 So it is normalized.

I will study the code that you suggest me.

Thk and best regards

Ok, I test normalization code from the example you suggest me:

gyro=[0.0533423312 1.1 0.012587996];
g=[1 -gyro(3) gyro(2); gyro(3) 1 -gyro(1); -gyro(2) gyro(1) 1];
dcmEst=dcmEst*g;
error=-dot(dcmEst(1,:),dcmEst(2,:))*0.5;
x_est=[0 0 0];
y_est=[0 0 0];
x_est = dcmEst(2, :) * error;
y_est = dcmEst(1, :) * error;
dcmEst(1,:) = dcmEst(1,:) + x_est;
dcmEst(2,:) = dcmEst(2,:) + y_est;
dcmEst(3,:) = cross(dcmEst(1,:), dcmEst(2,:));
renorm= dot(dcmEst(1,:),dcmEst(1,:));
if (renorm < 1.5625) && (renorm > 0.64)
renorm= .5 * (3-renorm);            %eq.21
elseif (renorm < 100.0) && (renorm > 0.01)
renorm= 1. / sqrt(renorm);
else
problem = TRUE;
end
dcmEst(1,:)=dcmEst(1,:)*renorm;

renorm= dot(dcmEst(2,:),dcmEst(2,:));
if (renorm < 1.5625) && (renorm > 0.64)
renorm= .5 * (3-renorm);            %eq.21
elseif (renorm < 100.0) && (renorm > 0.01)
renorm= 1. / sqrt(renorm);
else
problem = TRUE;
end
dcmEst(2,:)=dcmEst(2,:)*renorm;

renorm= dot(dcmEst(3,:),dcmEst(3,:));
if (renorm < 1.5625) && (renorm > 0.64)
renorm= .5 * (3-renorm);            %eq.21
elseif (renorm < 100.0) && (renorm > 0.01)
renorm= 1. / sqrt(renorm);
else
problem = TRUE;
end
dcmEst(3,:)=dcmEst(3,:)*renorm;
%dcmEst(1,:)=dcmEst(1,:)/norm(dcmEst(1,:));
%dcmEst(2,:)=dcmEst(2,:)/norm(dcmEst(2,:));
%dcmEst(3,:)=dcmEst(3,:)/norm(dcmEst(3,:));
gyro
dcmEst

and the result is the same as with my code.

But, I have a big doubt.

My gyro data is [0 2.5 0] it's mean that the IMU is rotating 2.5 deg/sec on one axis, right?

As I rotate on just one axis, only 4 values of DCM will change, rigth?

The change will be cos(2.5) or sin(2.5), rigth?

cos(2.5) is almost 1 and sin(2.5) is almost 0, so the change on dcm values will be litle, right?

Well, why I have a big change on dcm value when I apply a rotate of just 1.1 degree?

Am I asuming something wrong? or am I making a mistake on my matlab code?

Thk and best regards!

I'm still working on it trying to understand DCM.

Take a look at this code:

clear;
imu_interval=0.02;
dcmEst=[1 0 0; 0 1 0; 0 0 1];
gyro=[0 1 0] * imu_interval;
g=[1 -gyro(3) gyro(2); gyro(3) 1 -gyro(1); -gyro(2) gyro(1) 1];
graf(5000,2)=zeros;
for n = 1:5000
dcmEst=dcmEst*g;
error=-dot(dcmEst(1,:),dcmEst(2,:))*0.5;
x_est=[0 0 0];
y_est=[0 0 0];
x_est = dcmEst(2, :) * error;
y_est = dcmEst(1, :) * error;
dcmEst(1,:) = dcmEst(1,:) + x_est;
dcmEst(2,:) = dcmEst(2,:) + y_est;
dcmEst(3,:) = cross(dcmEst(1,:), dcmEst(2,:));
dcmEst(1,:)=dcmEst(1,:)/norm(dcmEst(1,:));
dcmEst(2,:)=dcmEst(2,:)/norm(dcmEst(2,:));
dcmEst(3,:)=dcmEst(3,:)/norm(dcmEst(3,:));
graf(n,1)=n*imu_interval;
graf(n,2)=dcmEst(1,1);

end
figure
plot(graf(:,1),graf(:,2));

In this case I put 1 deg/sec on my Y gyro, my timeStep is 20 msec. I run a loop for 0.02*5000 = 10 sec. I save DCM[0][0] and time to make a graph. This is what I obtain:

http://imageshack.us/a/img197/506/bj9i.jpg

As you can see DCM[0][0] is going from 1 to 0 to -1 to 0 to 1 several times while my gyro going from 0 to 10°... Is it ok?

I think that DCM[0][0] will going from 1 to 0 while gyro rotate from 0 to 90°... So is my asumtion right or is my graph right?

Regards!

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

William i think I express my self wrong. My english is not well.

I know that when I rotate my IMU DCM will not look as identity matrix.

What I tryin to say is, if I mesure only Y gyro then when I rotate the IMU from 0 to 180° then DCM[0][0] will go from 1 to -1 folloging cosine function. Right?

What I see on my test is that DCM[0][0] going from 1 to 0 when I rotate the IMU just a few degrees...

So reading on the web, studying again your paper I found that gyro data must be on rad/sec. I was working with °/sec,.

I change that and it is working well now.

So, thk a lot for your coment!

Regards! and sorry for my bad english... i think I need to take some clases...

Hi Leonardo,

I am glad it was something simple. I find that, in these sorts of discussions, that I should not make any assumptions. Your English is good enough, its just that describing this sort of thing is not easy.

Gyro units was top on my list of suspects, I am glad that is what it turned out to be.

Best regards, and happy flying,

Bill

By the way, William, do you know where do I find a simple GUI to see graphical DCM parameters? I see several times on youtube but I don't know where do I get it.

Best regards!!

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!

1

2

3

4

5

6

## Groups

45 members

1472 members

282 members

470 members

• ### 3D Models

57 members

Season Two of the Trust Time Trial (T3) Contest
A list of all T3 contests is here. The current round, the Vertical Horizontal one, is here