Untitled

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]));
}