单个传感器都有各自的局限性，传感器融合可以结合各传感器优点，得到更可靠的状态估计。本文介绍最常用的IMU+里程计融合方法。

一、传感器特性

　　编码器/里程计：短时间精度高，但累积误差随时间增大（每走1m可能误差几厘米）。

　　IMU陀螺仪：角速度测量瞬时精度高，但积分后漂移严重。即使高品质IMU，每秒也可能漂移1度以上。

　　IMU加速度计：可以计算俯仰和横滚角（静态时），但受运动加速度干扰大。

　　磁力计：提供绝对航向（地磁），但容易被环境磁场干扰。

二、互补滤波

　　互补滤波是最简单的融合算法，本质上是高通+低通滤波器组合：

// 互补滤波融合角度
// 陀螺仪积分（高通）+ 加速度计/磁力计（低通）
float alpha = 0.98;  // 滤波系数

float complementaryFilter(float gyroRate, float accAngle, float dt) {
  // gyroRate: 陀螺仪角速度（度/秒）
  // accAngle: 加速度计计算的角度（度）
  // dt: 采样时间间隔（秒）
  
  static float angle = 0;
  angle = alpha * (angle + gyroRate * dt) + (1-alpha) * accAngle;
  return angle;
}

　　alpha越大越信任陀螺仪（短期精度），alpha越小越信任加速度计（长期稳定）。典型值0.96-0.99。

三、卡尔曼滤波

　　卡尔曼滤波是更高级的融合算法，适用于线性高斯系统。一维卡尔曼滤波实现：

typedef struct {
  float x;      // 状态估计
  float P;      // 协方差
  float Q;      // 过程噪声
  float R;      // 测量噪声
} Kalman_t;

void kalmanInit(Kalman_t *k, float initX, float initP, float Q, float R) {
  k->x = initX;
  k->P = initP;
  k->Q = Q;
  k->R = R;
}

float kalmanUpdate(Kalman_t *k, float measurement) {
  // 预测
  k->P += k->Q;
  
  // 更新
  float K = k->P / (k->P + k->R);  // 卡尔曼增益
  k->x += K * (measurement - k->x);
  k->P = (1 - K) * k->P;
  
  return k->x;
}

　　Q和R需要根据实际传感器调试。Q越大表示"更新步的不确定性大"（更信任测量值），R越大表示"测量噪声大"（更信任预测值）。

四、MPU6050实战代码

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

MPU6050 mpu;
Kalman_t kalmanRoll, kalmanPitch;
float dt;

void setup() {
  Wire.begin();
  mpu.initialize();
  
  // 初始化卡尔曼滤波器
  kalmanInit(&kalmanRoll, 0, 1, 0.01, 0.1);
  kalmanInit(&kalmanPitch, 0, 1, 0.01, 0.1);
}

void loop() {
  // 读取IMU
  int16_t ax, ay, az, gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  
  // 加速度计计算俯仰和横滚
  float accRoll = atan2(ay, az) * 180 / PI;
  float accPitch = atan2(-ax, sqrt(ay*ay + az*az)) * 180 / PI;
  
  // 陀螺仪角速度（度/秒）
  float gyroRate = gx / 131.0;
  
  // 卡尔曼滤波融合
  float roll = kalmanUpdate(&kalmanRoll, accRoll);
  
  Serial.print("Roll: "); Serial.print(roll);
  Serial.print(" Gyro: "); Serial.println(gyroRate);
  
  delay(10);
}

五、常见问题

　　Q：角度漂移越来越严重？ A：陀螺仪的零偏误差在积分中不断累积。启动时应对陀螺仪进行静态校准，记录1000个样本取平均作为零偏值减去。

　　Q：加速度计读数杂乱？ A：加速度计对高频振动敏感。在软件中加低通滤波（如指数移动平均）或使用互补滤波中的低通部分。