I'm using the complimentary filter, like yours, but I'm using I2C sensors so the code is a bit different.

void gyroRead(float vals[])

{

//First data register

int regAddress = 0x1D;

int x, y, z;

byte buffer[6];

readFrom(addrGyro, regAddress, 6, buffer);

vals[0] = (int)(((((int)buffer[0]) << 8) | buffer[1])/14.375);

vals[1] = (int)(((((int)buffer[2])<< 8) | buffer[3])/14.375);

vals[2] = (int)(((((int)buffer[4]) << 8) | buffer[5])/14.375);

vals[0] *= (-1);

vals[2] *= (-1);

}

{

//First data register

int regAddress = 0x32;

byte buffer[6];

readFrom(addrAccel, regAddress, 6, buffer);

vals[0] = ((((int)buffer[1]) << 8) | buffer[0]);

vals[1] = ((((int)buffer[3])<< 8) | buffer[2]);

vals[2] = ((((int)buffer[5]) << 8) | buffer[4]);

vals[0] /= xAccSens;

vals[1] /= yAccSens;

vals[2] /= zAccSens;

vals[0] *= (-1);

vals[2] *= (-1);

}

void kalman(float accelVals[2], float gyroRate[2], float compAngle[2])

{

//Vector for tilt from accelerometer only

float r = sqrt((pow(accelVals[0], 2) + pow(accelVals[1], 2) + pow(accelVals[2], 2)));

//Angles from x,y,z axis to R

vectorAngle[0] = acos(accelVals[0]/r) * (180/pi);

vectorAngle[1] = acos(accelVals[1]/r) * (180/pi);

deltatime = millis()-time;

time = millis();

//Computed angle using accelerometer data and gyroscope rate of change.

compAngle[0] = (0.98*(compAngle[0]+(gyroRate[0]*deltatime/1000)))+(0.02*(vectorAngle[0]));

compAngle[1] = (0.98*(compAngle[1]+(gyroRate[1]*deltatime/1000)))+(0.02*(vectorAngle[1]));

}

accelVals array stores the x and y acceleration in g's

gyroRate is the x and y rotation in degrees/sec

vectorAngle is the angle between the axis and the R vector

I was thinking it may have something to do with the frequency of the loop. When I increase the frequency, the problem is not as severe. Unfortunately there is only so fast I can make it.

I've tried various values for the weighted average. There isn't really much margin for change from what I can see before bad things start happening.

Another idea I just had....I may be doing some funny things with the sign (positive/negative) of various angles and rates. I will look into this.

I'm not sure if I'm missing something here or not.