Guide to gyro and accelerometer with Arduino including Kalman filtering

Using Arduino Sensors
75

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.