I have been attempting to use my GY-85 w/ ITG3205 Gyro, ADXL345 Accelerometer (not currently using mag) to calculate the relative angle of a quadcopter. The biggest issue I'm having is, I believe, the drift from the gyro. I implemented a complementary filter between the gyro and accelerometer readings, but I have not been able to get the calculated angles to stop changing drastically over time. Even changing the offset values does not get me close enough. At the same time, if I shake and rotate the IMU, when I set it back to a level position, oft times the gyro angles, and thus the calculated over-all angles, will be drastically off for several minutes.

I have read that the Kalman filter can help, but I do not yet understand it. Is there a better way to accomplish truly accurate readings? Or am I simply doing something wrong?

Modification: I tossed in to if() statements before the final angle calculations, and it seems to be fairly accurate. Is there anything overly wrong with these? I know it's a very simple fix, and I don't know if it will have adverse affects later on. I have included the statements in the code.

```
void updateSensors(){
getAccel();
getGyro();
getMag();
if (accAngle[0] > -5 && accAngle[0] < 5) gyroAngle[0] = 0;
if (accAngle[1] > -5 && accAngle[1] < 5) gyroAngle[1] = 0;
calcAngle[0] = .90*gyroAngle[0]+(1-.90)*accAngle[0]; //Complementary filter for calculating angle from both gyro and acc data
calcAngle[1] = .90*gyroAngle[1]+(1-.90)*accAngle[1];
}
void sensorInit(){
writeTo(accelDev,0x2D, 0); //Writes to different sensor registers to set power modes, filters, etc... see data sheets for ADXL345, ITG3205, HMC5883L
writeTo(accelDev, 0x2D, 16);
writeTo(accelDev, 0x2D, 8);
writeTo(gyroDev, 0x3E, 0x81); //Sets CLK_SEL=1, enabling PLL with X Gyro reference for clock source, sets H_RESET=1, resetting device to default settings
writeTo(gyroDev, 0x15, 0x07); //Sets the sampling rate in SMPLRT_DIV=7, and gives F=1khz/(7+1) = 125 Hz, or 8 ms per sample
writeTo(gyroDev, 0x16, 0x1A); //Sets FS_SEL=3, which gives the max range of +/- 2000 deg/s. DLPF_CFG = 2, sets LPF Bandwidth to 98 Hz and Internal Sample Rate to 1 kHz
writeTo(magDev, 0x02, 0x00);
}
void getAccel(){
if ((micros() - nta) >= 2500){ //Only update every 2500 microseconds, ~400 Hz rate
int regAddress = 0x32;
readFrom(accelDev, regAddress, 6, buffAcc); //Starting at the first data register, reades the bytes for x, y, z axis successively
accX = ((((int)buffAcc[1]) << 8) | buffAcc[0]) + acc_offx; //Shifts the first byte left 8 places and concatenates first byte with second to get 16-bit sensor value
accY = ((((int)buffAcc[3]) << 8) | buffAcc[2]) + acc_offy;
accZ = ((((int)buffAcc[5]) << 8) | buffAcc[4]) + acc_offz;
accAngle[1] = -(atan2(accX, accZ))*radToDeg; //Y-axis angle, Roll
accAngle[0] = (atan2(accY, accZ))*radToDeg; //X-axis angle, Pitch
nta = micros();
}
}
void getGyro(){
if ((micros() - ntg) >= 8000){ //Only update every 8000 microseconds, ~125 Hz rate
int regAddress = 0x1B;
readFrom(gyroDev, regAddress, 8, buffGyro);
gyrox_temp = ((((int)buffGyro[2]) << 8) | buffGyro[3]) + g_offx;
gyroy_temp = ((((int)buffGyro[4]) << 8) | buffGyro[5]) + g_offy;
gyroz_temp = ((((int)buffGyro[6]) << 8) | buffGyro[7]) + g_offz;
//temp = (((int)buffGyro[0]) << 8) | buffGyro[1];
double dt = (micros() - ntg)/1000000.00; //To calculate the time since the last measurement
gyroX = (double)gyrox_temp*dt/14.375; //Per the data sheet, sensitivity scale factor per LSB/(deg/sec)
gyroY = (double)gyroy_temp*dt/14.375;
gyroZ = (double)gyroz_temp*dt/14.375;
gyroAngle[0] += gyroX;
gyroAngle[1] += gyroY;
gyroAngle[2] += gyroZ;
ntg = micros();
}
}
```