Electronic – 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

  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;


  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?

Best Answer

(This question has been posted more than a year ago)

You need these angles in order to apply a control, a PID for example, to stabilize the quadcopter in hovering mode. Your control is supposed to be active when you are not remote controlling your drone and you want it to be perfectly horizontal. This means that the theta(x axis) and phi(y axis) angles will be close to zero and will reach the desired reference value ( =0 ) thanks to the control.

I have a question for you: Have you recorded these values while the motors were spinning? I've noticed an important increase in noise amplitude caused by vibrations on the chassis.