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.