mpu9250卡尔曼滤波姿态解算代码

MPU9250是一种常用的九轴传感器模块，可以同时测量加速度、陀螺仪和磁力计数据。卡尔曼滤波是一种常用的姿态解算算法，可以通过融合加速度计和陀螺仪数据来估计物体的姿态。

以下是一个简单的MPU9250卡尔曼滤波姿态解算的代码示例：

#include <Wire.h>
#include <MPU9250.h>
#include <MahonyAHRS.h>

MPU9250 imu;
Mahony filter;

void setup() {
  Wire.begin();
  imu.setup();
  imu.setAccelRange(MPU9250::ACCEL_RANGE_2G);
  imu.setGyroRange(MPU9250::GYRO_RANGE_250DPS);
  imu.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_20HZ);
  imu.setSrd(19);
  filter.begin(100);
}

void loop() {
  imu.update();
  filter.update(imu.getAccelX(), imu.getAccelY(), imu.getAccelZ(), imu.getGyroX(), imu.getGyroY(), imu.getGyroZ());
  
  float roll, pitch, yaw;
  filter.getEuler(&roll, &pitch, &yaw);
  
  // 输出姿态角
  Serial.print("Roll: ");
  Serial.print(roll);
  Serial.print(" Pitch: ");
  Serial.print(pitch);
  Serial.print(" Yaw: ");
  Serial.println(yaw);
  
  delay(10);
}

这段代码使用了MPU9250库和MahonyAHRS库来读取传感器数据并进行卡尔曼滤波姿态解算。首先在`setup()`函数中初始化传感器，并设置加速度计和陀螺仪的量程、数字低通滤波器带宽和采样率分频。然后在`loop()`函数中更新传感器数据，并调用卡尔曼滤波器的`update()`函数进行姿态解算。最后通过`getEuler()`函数获取姿态角，并输出到串口。