Restrict either the roll or pitch as explained here: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf

examples/MPU6050/MPU6050.ino

@@ -18,19 +18,19 @@
 #include <Wire.h>
 #include "Kalman.h" // Source: https://github.com/TKJElectronics/KalmanFilter
 
+#define RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf
+
 Kalman kalmanX; // Create the Kalman instances
 Kalman kalmanY;
 
 /* IMU Data */
-int16_t accX, accY, accZ;
+double accX, accY, accZ;
+double gyroX, gyroY, gyroZ;
 int16_t tempRaw;
-int16_t gyroX, gyroY, gyroZ;
 
-double accXangle, accYangle; // Angle calculate using the accelerometer
-double temp; // Temperature
-double gyroXangle, gyroYangle; // Angle calculate using the gyro
-double compAngleX, compAngleY; // Calculate the angle using a complementary filter
-double kalAngleX, kalAngleY; // Calculate the angle using a Kalman filter
+double gyroXangle, gyroYangle; // Angle calculate using the gyro only
+double compAngleX, compAngleY; // Calculated angle using a complementary filter
+double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter
 
 uint32_t timer;
 uint8_t i2cData[14]; // Buffer for I2C data
@@ -57,20 +57,27 @@ void setup() {
 
   /* Set kalman and gyro starting angle */
   while (i2cRead(0x3B, i2cData, 6));
-  accX = ((i2cData[0] << 8) | i2cData[1]);
-  accY = ((i2cData[2] << 8) | i2cData[3]);
-  accZ = ((i2cData[4] << 8) | i2cData[5]);
+  accX = (i2cData[0] << 8) | i2cData[1];
+  accY = (i2cData[2] << 8) | i2cData[3];
+  accZ = (i2cData[4] << 8) | i2cData[5];
+
+  // Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
   // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
-  // We then convert it to 0 to 2π and then from radians to degrees
-  accYangle = (atan2(accX, accZ) + PI) * RAD_TO_DEG;
-  accXangle = (atan2(accY, accZ) + PI) * RAD_TO_DEG;
+  // It is then converted from radians to degrees
+#ifdef RESTRICT_PITCH // Eq. 25 and 26
+  double roll  = atan2(accY, accZ) * RAD_TO_DEG;
+  double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
+#else // Eq. 28 and 29
+  double roll  = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
+  double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
+#endif
 
-  kalmanX.setAngle(accXangle); // Set starting angle
-  kalmanY.setAngle(accYangle);
-  gyroXangle = accXangle;
-  gyroYangle = accYangle;
-  compAngleX = accXangle;
-  compAngleY = accYangle;
+  kalmanX.setAngle(roll); // Set starting angle
+  kalmanY.setAngle(pitch);
+  gyroXangle = roll;
+  gyroYangle = pitch;
+  compAngleX = roll;
+  compAngleY = pitch;
 
   timer = micros();
 }
@@ -78,34 +85,72 @@ void setup() {
 void loop() {
   /* Update all the values */
   while (i2cRead(0x3B, i2cData, 14));
-  accX = ((i2cData[0] << 8) | i2cData[1]);
-  accY = ((i2cData[2] << 8) | i2cData[3]);
-  accZ = ((i2cData[4] << 8) | i2cData[5]);
-  tempRaw = ((i2cData[6] << 8) | i2cData[7]);
-  gyroX = ((i2cData[8] << 8) | i2cData[9]);
-  gyroY = ((i2cData[10] << 8) | i2cData[11]);
-  gyroZ = ((i2cData[12] << 8) | i2cData[13]);
+  accX = ((i2cData[0] << 8) | i2cData[1]) + 2000.0;
+  accY = ((i2cData[2] << 8) | i2cData[3]) + 3319.84;
+  accZ = ((i2cData[4] << 8) | i2cData[5]) + 664.48;
+  tempRaw = (i2cData[6] << 8) | i2cData[7];
+  gyroX = (i2cData[8] << 8) | i2cData[9];
+  gyroY = (i2cData[10] << 8) | i2cData[11];
+  gyroZ = (i2cData[12] << 8) | i2cData[13];
+
+  double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
+  timer = micros();
 
+  // Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
   // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
-  // We then convert it to 0 to 2π and then from radians to degrees
-  accXangle = (atan2(accY, accZ) + PI) * RAD_TO_DEG;
-  accYangle = (atan2(accX, accZ) + PI) * RAD_TO_DEG;
-
-  double gyroXrate = (double)gyroX / 131.0;
-  double gyroYrate = -((double)gyroY / 131.0);
-  gyroXangle += gyroXrate * ((double)(micros() - timer) / 1000000); // Calculate gyro angle without any filter
-  gyroYangle += gyroYrate * ((double)(micros() - timer) / 1000000);
-  //gyroXangle += kalmanX.getRate()*((double)(micros()-timer)/1000000); // Calculate gyro angle using the unbiased rate
-  //gyroYangle += kalmanY.getRate()*((double)(micros()-timer)/1000000);
-
-  compAngleX = (0.93 * (compAngleX + (gyroXrate * (double)(micros() - timer) / 1000000))) + (0.07 * accXangle); // Calculate the angle using a Complimentary filter
-  compAngleY = (0.93 * (compAngleY + (gyroYrate * (double)(micros() - timer) / 1000000))) + (0.07 * accYangle);
-
-  kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros() - timer) / 1000000); // Calculate the angle using a Kalman filter
-  kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros() - timer) / 1000000);
-  timer = micros();
+  // It is then converted from radians to degrees
+#ifdef RESTRICT_PITCH // Eq. 25 and 26
+  double roll  = atan2(accY, accZ) * RAD_TO_DEG;
+  double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
+#else // Eq. 28 and 29
+  double roll  = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
+  double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
+#endif
+
+  double gyroXrate = gyroX / 131.0; // Convert to deg/s
+  double gyroYrate = gyroY / 131.0; // Convert to deg/s
+
+#ifdef RESTRICT_PITCH
+  // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
+  if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
+    kalmanX.setAngle(roll);
+    compAngleX = roll;
+    kalAngleX = roll;
+    gyroXangle = roll;
+  } else
+    kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
+
+  if (abs(kalAngleX) > 90)
+    gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
+  kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
+#else
+  // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
+  if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
+    kalmanY.setAngle(pitch);
+    compAngleY = pitch;
+    kalAngleY = pitch;
+    gyroYangle = pitch;
+  } else
+    kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter
+
+  if (abs(kalAngleY) > 90)
+    gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading
+  kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
+#endif
 
-  temp = ((double)tempRaw + 12412.0) / 340.0;
+  gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
+  gyroYangle += gyroYrate * dt;
+  //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate
+  //gyroYangle += kalmanY.getRate() * dt;
+
+  compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter
+  compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;
+
+  // Reset the gyro angle when it has drifted too much
+  if (gyroXangle < -180 || gyroXangle > 180)
+    gyroXangle = kalAngleX;
+  if (gyroYangle < -180 || gyroYangle > 180)
+    gyroYangle = kalAngleY;
 
   /* Print Data */
 #if 0 // Set to 1 to activate
@@ -116,21 +161,29 @@ void loop() {
   Serial.print(gyroX); Serial.print("\t");
   Serial.print(gyroY); Serial.print("\t");
   Serial.print(gyroZ); Serial.print("\t");
+
+  Serial.print("\t");
 #endif
-  Serial.print(accXangle); Serial.print("\t");
+
+  Serial.print(roll); Serial.print("\t");
   Serial.print(gyroXangle); Serial.print("\t");
   Serial.print(compAngleX); Serial.print("\t");
   Serial.print(kalAngleX); Serial.print("\t");
 
   Serial.print("\t");
 
-  Serial.print(accYangle); Serial.print("\t");
+  Serial.print(pitch); Serial.print("\t");
   Serial.print(gyroYangle); Serial.print("\t");
   Serial.print(compAngleY); Serial.print("\t");
   Serial.print(kalAngleY); Serial.print("\t");
 
-  //Serial.print(temp);Serial.print("\t");
+#if 0 // Set to 1 to print the temperature
+  Serial.print("\t");
+
+  double temperature = (double)tempRaw / 340.0 + 36.53;
+  Serial.print(temperature); Serial.print("\t");
+#endif
 
   Serial.print("\r\n");
-  delay(1);
+  delay(2);
 }