11.卡爾曼濾波

Kalman kalmanX , kalmanY, kalmanZ;

MPU6050 accelgyro;

int16_t ax, ay, az, gx, gy, gz;

double accX, accY, accZ, gyroX, gyroY, gyroZ;

uint32_t timer;

double pitch, roll, yaw, AngleX, AngleY, AngleZ, Xrate, Yrate, Zrate;

void setup() {

Wire.begin();

Serial.begin(115200);

accelgyro.initialize();

accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

accX = ax; accY = ay; accZ = az; gyroX = gx; gyroY = gy; gyroZ = gz;

pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;

roll = atan2(accY , accZ) * RAD_TO_DEG;

yaw = atan2(accX , accY) * RAD_TO_DEG;

kalmanY.setAngle(pitch);

kalmanX.setAngle(roll);

kalmanZ.setAngle(yaw);

}

void loop() {

accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

accX = ax;

accY = ay;

accZ = az;

gyroX = gx;

gyroY = gy;

gyroZ = gz;

double dt = (double)(micros() - timer) / 1000000; // Calculate delta time

timer = micros();

pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;

roll = atan2(accY,accZ) * RAD_TO_DEG;

yaw = atan2(accX , accY) * RAD_TO_DEG;

Yrate = gyroY / 131.0; // Convert to deg/s

Xrate = gyroX / 131.0;

Zrate = gyroZ / 131.0;

AngleY = kalmanY.getAngle(pitch, Yrate, dt)+2;

AngleX = int(kalmanX.getAngle(roll, Xrate, dt));

AngleZ = int(kalmanZ.getAngle(yaw, Zrate, dt));

Serial.print(AngleX);

Serial.print(",");

Serial.print(AngleY);

Serial.print(",");

Serial.println(AngleZ);

}