mpu6050卡尔曼滤波代码

当然，我可以为您提供MPU6050卡尔曼滤波的代码示例。以下是一个基于Arduino的示例代码：

#include <Wire.h>
#include <MPU6050.h>

MPU6050 mpu;

// 卡尔曼滤波参数
float angle;  // 卡尔曼滤波后的角度
float Q_angle = 0.001;  // 角度测量噪声协方差
float Q_bias = 0.003;  // 角速度测量噪声协方差
float R_measure = 0.03;  // 角度测量误差协方差

float P[2][2] = {{1, 0}, {0, 1}};  // 状态协方差
float K[2];  // 卡尔曼增益

float dt;  // 循环周期
unsigned long prevTime;

void setup() {
  Serial.begin(9600);
  
  Wire.begin();
  mpu.initialize();
  
  // 获取初始角度
  float accX = mpu.getAccelerationX();
  float accY = mpu.getAccelerationY();
  float accZ = mpu.getAccelerationZ();
  
  angle = atan2(accY, accZ) * (180 / M_PI);
  
  prevTime = micros();
}

void loop() {
  unsigned long currentTime = micros();
  dt = (currentTime - prevTime) / 1000000.0;
  
  float gyroRateX = mpu.getRotationX() / 131.0;
  float gyroRateY = mpu.getRotationY() / 131.0;
  float gyroRateZ = mpu.getRotationZ() / 131.0;
  
  float accX = mpu.getAccelerationX();
  float accY = mpu.getAccelerationY();
  float accZ = mpu.getAccelerationZ();
  
  // 计算角度
  float accAngle = atan2(accY, accZ) * (180 / M_PI);
  
  // 卡尔曼滤波
  float rate = gyroRateX - angle;
  angle += dt * rate;
  
  P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
  P[0][1] -= dt * P[1][1];
  P[1][0] -= dt * P[1][1];
  P[1][1] += Q_bias * dt;
  
  float S = P[0][0] + R_measure;
  
  K[0] = P[0][0] / S;
  K[1] = P[1][0] / S;
  
  angle += K[0] * (accAngle - angle);
  angle += K[1] * rate;
  
  P[0][0] -= K[0] * P[0][0];
  P[0][1] -= K[0] * P[0][1];
  P[1][0] -= K[1] * P[0][0];
  P[1][1] -= K[1] * P[0][1];
  
  // 更新时间
  prevTime = currentTime;
  
  Serial.println(angle);
}

请注意，这只是一个简单的示例代码，您可能需要根据您的具体需求进行调整和优化。此代码假设您已经正确连接了MPU6050并且已经配置了I2C通信。您还需要安装MPU6050库以使此代码正常工作。

希望这可以帮到您！如果您有任何其他问题，请随时问我。