KalmanFilter_MPU6050 example #1
Description
Hello,
When I'm putting "delay" at the end of the loop, for example:
void loop()
{
Vector acc = mpu.readNormalizeAccel();
Vector gyr = mpu.readNormalizeGyro();
// Calculate Pitch & Roll from accelerometer (deg)
accPitch = -(atan2(acc.XAxis, sqrt(acc.YAxis_acc.YAxis + acc.ZAxis_acc.ZAxis))_180.0)/M_PI;
accRoll = (atan2(acc.YAxis, acc.ZAxis)_180.0)/M_PI;
// Kalman filter
kalPitch = kalmanY.update(accPitch, gyr.YAxis);
kalRoll = kalmanX.update(accRoll, gyr.XAxis);
Serial.print(accPitch);
Serial.print(":");
Serial.print(accRoll);
Serial.print(":");
Serial.print(kalPitch);
Serial.print(":");
Serial.print(kalRoll);
Serial.print(":");
Serial.print(acc.XAxis);
Serial.print(":");
Serial.print(acc.YAxis);
Serial.print(":");
Serial.print(acc.ZAxis);
Serial.print(":");
Serial.print(gyr.XAxis);
Serial.print(":");
Serial.print(gyr.YAxis);
Serial.print(":");
Serial.print(gyr.ZAxis);
Serial.println();
delay(50);
}
I get result, even if I didn't move the device:
0.34:0.54:0.89:0.58:-0.05:0.08:8.88:-0.11:0.03:-0.03
-0.20:0.59:-0.30:0.55:0.03:0.09:8.87:0.07:0.03:-0.09
0.02:0.34:-0.01:0.39:-0.00:0.05:8.90:0.13:-0.22:0.03
-0.35:0.91:-0.27:0.85:0.05:0.14:8.74:0.07:0.03:0.03
0.33:0.48:0.17:0.56:-0.05:0.07:8.81:0.19:0.03:0.03
-0.31:1.09:0.03:0.98:0.05:0.17:8.80:0.01:0.21:0.03
0.08:0.67:-0.64:0.83:-0.01:0.10:8.78:0.07:0.09:0.09
0.02:0.85:2.07:0.62:-0.00:0.13:8.88:-0.30:0.09:-0.03
-0.15:0.81:-20.49:1.16:0.02:0.13:8.94:-0.18:0.09:0.03
-0.36:0.47:-108.53:-0.10:0.06:0.07:8.84:0.13:0.03:0.03
-0.23:0.46:273.79:1.44:0.04:0.07:8.91:-0.11:0.03:-0.03
-0.34:0.85:-1636.03:-0.95:0.05:0.13:8.89:0.07:-0.10:-0.03
0.06:1.01:-8819.24:4.53:-0.01:0.16:8.93:0.07:-0.03:0.09
0.08:1.05:25352.41:-6.24:-0.01:0.16:8.86:-0.11:-0.03:0.03
0.06:0.81:-267706.43:16.90:-0.01:0.12:8.84:0.07:0.09:0.15
-0.44:0.81:-1461554.25:-37.03:0.07:0.12:8.77:0.01:-0.03:0.09
0.05:0.47:3979674.00:95.37:-0.01:0.07:8.78:-0.18:0.09:0.09
-0.03:0.67:-27589968.00:-253.84:0.00:0.10:8.77:-0.30:0.15:0.03
0.34:0.81:-135436864.00:732.94:-0.05:0.12:8.85:-0.05:0.03:-0.03
0.14:0.88:599838336.00:-2266.80:-0.02:0.14:8.84:0.13:0.03:-0.03
-0.17:0.91:ovf:7601.97:0.03:0.14:8.76:-0.11:0.03:-0.09
-0.12:0.66:ovf:-27740.25:0.02:0.10:8.88:-0.11:-0.16:0.03
-0.32:1.04:2811924480.00:111337.50:0.05:0.16:8.87:0.01:0.09:-0.03
-0.13:0.49:-2122166272.00:-497866.00:0.02:0.07:8.75:-0.11:-0.16:0.03
-0.25:1.00:1911468032.00:2521112.00:0.04:0.16:8.90:-0.11:0.09:-0.03
-0.03:0.87:-2042695680.00:-14847424.00:0.00:0.13:8.86:0.01:0.15:0.09
0.22:0.67:2615058432.00:106341120.00:-0.03:0.10:8.85:-0.11:0.09:0.03
-0.27:0.70:-4098588672.00:-1011144704.00:0.04:0.11:8.76:-0.30:0.21:0.03
0.22:0.56:ovf:ovf:-0.03:0.09:8.86:-0.11:0.15:0.09
0.03:0.53:ovf:ovf:-0.00:0.08:8.83:-0.05:0.15:0.03
Can you help me, what I need to change?
Br
Piotr