# Angle errors in IMU (using Accl & Gyro)

I am making Autonomous Quad. I am programming my IMU using ADXL335 -3 Axis analog Accelerometer & L3G4200D 3-axis digital Gyro. I am using ARM Cortex M4F based TI's stellaris launchpad LM4F120XL.

I've a problem while implementing IMU. Here is my algorithm of IMU...

By ADC I am getting reading of accelerometer in Dx, Dy & Dz. Now as I've 12bit ADC I am using the following formula to get the readings in "g"

Rx = [(Dx*(3.3/4095) - Tx] / 0.3
Ry = [(Dy*(3.3/4095) - Ty] / 0.3
Rz = [(Dz*(3.3/4095) - Tz] / 0.3


In stable condition I've checked the Tx, Ty, Tz:

Tx = 1.630820
Ty = 1.658477
Tz = 1.997728


For Gyro (at 250 dps with 8.75 mdps/digit)

To get reading in dps I am using following formula

X = Gx*8.75/1000
Y = Gy*8.75/1000
Z = Gz*8.75/1000


(Here Gx, Gy & Gz are data from gyro... I've checked them- they are signed values and also accurate (min. noise) )

I want to measure angles with respect to all axis (pitch, roll & yaw). For that I am doing as per following

Th = 5;
T = 0.05; // approx. 5ms I've checked it using counter
c = 180/3.14;

while (1) {
// initial values of Rxe, Rye, Rze are same as Rx, Ry, Rz
Xd = atan2(Rxe, sqrt( (Rze*Rze) + (Rye*Rye) ));
Yd = atan2(Rye, sqrt( (Rze*Rze) + (Rxe*Rxe) ));
Zd = atan2(Rze, sqrt( (Rye*Rye) + (Rxe*Rxe) ));

// To calculate Xavg, Yavg, Zavg from two consecutive X, Y, Z values respectively
gyro();

if ((Xavg >= Th) || (Xavg <= Th)) {
Xd = Xd + (Xavg*T);
}
if ((Yavg >= Th) || (Yavg <= Th)) {
Yd = Yd + (Yavg*T);
}
if ((Zavg >= Th) || (Zavg <= Th)) {
Zd = Zd + (Zavg*T);
}

Rxg = sin(Yd) / (sqrt(1 + ((cos(Yd) * cos(Yd)) * (tan(Xd) * tan(Xd)))));
Ryg = sin(Xd) / (sqrt(1 + ((cos(Xd) * cos(Xd)) * (tan(Yd) * tan(Yd)))));
Rzg = ((Rzg > 0) ? 1 : ((Rzg < 0) ? -1 : 1))*(sqrt(1 - (Rxg*Rxg)-(Ryg*Ryg)));

get_accl_data();            // To get ADC values
accelerometer();            // To convert ADC valuse in "g" - Rx, Ry, Rz

Rxe = (Rx*0.015)+(Rxg*0.985);      // weighted average
Rye = (Ry*0.015)+(Ryg*0.985);
Rze = (Rz*0.015)+(Rzg*0.985);

Xd *= c;     // converting in degree
Yd *= c;
Zd *= c;

Print(Xd);
Print(Xd);
Print(Xd);

Xd /*=c;    // converting in radians again
Yd /*=c;
Zd /*=c;
}


I am printing Xd, Yd, Zd... It is okay while satble or within +/- 10 degree... But for more than 10 degree I am just getting fault values. I mean It is informing whether the device is satable or not OR whether it is moving on positive side(counter clock wise) or negative side (clockwise) BUT not giving the exact +/- 40 degree output while it is actually on 40 angle with respect to ground!

What I am missing? Please help me ... I've done it by referring some docs and blogs.

// al variables type are taken proper (as per my knowledge).

One more question : Do I really need to know these angles? or can I use the current values also to make my quad stable ? What else data I'll require to make my quad stable?

• You need to include the variable type declarations in your code excerpt, as choices inconsistent with your operations could be a problem. Mar 22, 2013 at 11:25
• @Jigar4Electronics I suspect you are not receiving any answers because your question is too narrow, specific to a set of devices that may not be available to other members, and is not of sufficiently general interest to merit additional research effort by said members. Please consider whether you can slice the question down a bit. Mar 22, 2013 at 11:39
• Link to the data sheet, please. Apr 3, 2013 at 18:11
• For an IMU, a bit of math is inescapable. Have you written down what you want to achieve in terms of equations? You can use LaTeX on this site to communicate your intent. IMUs are a little more complex behind the scenes and the processing is somewhat more involved when it comes to fusing gyros with accelerometer data. Oct 3, 2014 at 6:03