在上一篇文章中，我们介绍了 MPU6050 输出的三轴加速度和三轴角速度原始数据，以及如何从这些数据中估算设备的姿态角度。然而，原始数据往往包含噪声与漂移，如果直接使用，角度会出现严重的抖动或长期偏移。因此，滤波器是 IMU 姿态解算中不可或缺的一环。

这篇文章就以 MPU6050 为例，重点介绍三种常用的滤波方法：

• 卡尔曼滤波器（Kalman Filter）

• 互补滤波器（Complementary Filter）

• 四元数滤波（Quaternion Filter）

并结合实际代码示例，帮大家搞懂它们是如何工作、各自适用场景及优缺点。

一、卡尔曼滤波器

1.1 卡尔曼滤波器的核心思想

卡尔曼滤波是一种基于贝叶斯估计的递推滤波算法，核心就是把预测值（由数学模型推导）和观测值（由传感器测量）结合起来，用噪声协方差来权衡谁更可信。

以 MPU6050 姿态角度为例：

• 状态变量：角度（angle）和陀螺仪偏差（bias）

• 预测值：上一时刻的角度 + 陀螺仪角速度积分得到的增量（这是因为由积分得到的角度，是计算出来的，所以叫做预测值）

• 观测值：加速度计测量的角度（从重力方向换算，算实际物理的角度，上一篇博客讲了怎么计算的，）

• 输入量：

  • 陀螺仪测量角速度

  • 加速度计估算角度

  • 过程噪声协方差（Q）

  • 测量噪声协方差（R）

1.2 关键点（理解重点）

• 预测步骤：用陀螺仪角速度积分预测角度

• 更新步骤：用加速度计角度矫正预测值

• 卡尔曼增益自动计算谁更可信（这个信任度是可以自动改变的，这是卡尔曼的精妙之处）
  （与手动分配一个固定权重（比如互补滤波里的 α，下面会介绍）相比，卡尔曼滤波的增益是自动、动态变化的，每个时刻都会根据实际情况重新计算，始终找到当前最优的信任度。）

  名称	作用	是否动态

  Q

  过程噪声协方差（先验设定）

  固定

  R

  观测噪声协方差（先验设定）

  固定

  P

  状态估计的误差协方差（自适应估计）

  动态变化

  K

  卡尔曼增益

  每次根据 P 和 R 动态计算

  卡尔曼滤波里面有四个关键的参数分别为Q（过程协方差）、R（预测协方差）、P（协方差矩阵）、K（根据前面3个参数计算得出）
  Q、R、P这三个参数是卡尔曼滤波器的核心参数，直接决定了卡尔曼滤波的灵敏度和信任权重。
  （1）过程噪声协方差（Q） 和 观测噪声协方差（R） 的分量，是固定的参数（除非自适应滤波），Q、R 是先验的“外部”噪声约束，告诉滤波器“我认为过程模型有多好/观测有多准”。（这个值在下方代码的时候进行了初始化）
  （2）P 是动态的，随每次预测、更新调整，它在每一次预测和更新时都会被重新计算，反映当前状态的不确定性是随着新数据而不断变化的。它是内部动态“实时估计的置信度”，表示当前状态估计到底有多准。所以P 的初始值确实不影响最终的滤波效果，但会影响滤波器前期收敛的速度和表现。最终稳态值就是 Q 和 R 的函数，总结一句话，状态协方差 P 的最终稳态值完全由系统的 Q、R 和矩阵模型决定！
  （3）因为卡尔曼增益 K 是由状态协方差矩阵 P 和观测噪声协方差 R 决定的，而 P又会动态演化，且它的演化与过程噪声协方差 Q 有关，而 Q、 是固定参数。因此，滤波器的效果好坏，主要取决于对 Q、R 的合理设置。
  根据经验得出来的

  MPU6050 常用参数（经验）：

  float Q_angle = 0.001f;  // 过程噪声（角度）
  float Q_bias  = 0.003f;  // 过程噪声（陀螺仪偏置）
  float R_measure = 0.03f; // 观测噪声（加速度计角度）

• 如果滤波反应慢，可能 R_measure 太小或 Q_angle 太小（预测太自信）。

• 如果滤波抖动大，可能 R_measure 太小（太相信观测）或 Q_angle 太大（太不信预测）。

1.3 MPU6050 角度卡尔曼滤波示例（C语言） 

#include <stdio.h>
#include <math.h>

// 卡尔曼滤波结构体，保存状态和协方差矩阵
typedef struct {
    float angle;    // 角度估计值
    float bias;     // 陀螺仪零偏估计值
    float P[2][2];  // 状态误差协方差矩阵，2x2维度
} Kalman_t;

// 初始化卡尔曼滤波器
// 输入：kf指针，初始角度init_angle
void Kalman_Init(Kalman_t* kf, float init_angle) {
    kf->angle = init_angle;      // 初始化角度
    kf->bias = 0.0f;            // 初始化偏置为0
    // 初始化协方差矩阵P为单位矩阵，表示初始不确定度
    kf->P[0][0] = 1.0f; kf->P[0][1] = 0.0f;
    kf->P[1][0] = 0.0f; kf->P[1][1] = 1.0f;
}

// 卡尔曼滤波更新函数
// 输入：kf指针，陀螺仪角速度gyroRate，加速度计计算角度accelAngle，采样时间dt
//       过程噪声Q_angle、Q_bias，测量噪声R_measure
// 返回：更新后的角度估计
float Kalman_Update(Kalman_t* kf, float gyroRate, float accelAngle, float dt,
                    float Q_angle, float Q_bias, float R_measure) {
    // ===== 预测步骤 =====
    // 陀螺仪去除零偏后的角速度
    float rate = gyroRate - kf->bias;
    // 利用陀螺仪积分预测角度
    kf->angle += dt * rate;

    // 预测协方差矩阵P的更新，考虑过程噪声Q
    kf->P[0][0] += dt * (dt * kf->P[1][1] - kf->P[0][1] - kf->P[1][0] + Q_angle);
    kf->P[0][1] -= dt * kf->P[1][1];
    kf->P[1][0] -= dt * kf->P[1][1];
    kf->P[1][1] += Q_bias * dt;

    // ===== 计算卡尔曼增益 =====
    float S = kf->P[0][0] + R_measure;    // 估计误差协方差 + 观测噪声协方差
    float K0 = kf->P[0][0] / S;           // 卡尔曼增益的第一个分量
    float K1 = kf->P[1][0] / S;           // 卡尔曼增益的第二个分量

    // ===== 更新估计 =====
    float y = accelAngle - kf->angle;     // 观测值与预测值的误差
    kf->angle += K0 * y;                  // 角度更新
    kf->bias += K1 * y;                   // 零偏更新

    // ===== 更新协方差矩阵 =====
    float P00_temp = kf->P[0][0];
    float P01_temp = kf->P[0][1];
    kf->P[0][0] -= K0 * P00_temp;
    kf->P[0][1] -= K0 * P01_temp;
    kf->P[1][0] -= K1 * P00_temp;
    kf->P[1][1] -= K1 * P01_temp;

    return kf->angle; // 返回最新的角度估计
}

// 示例主程序
int main() {
    Kalman_t kf;
    Kalman_Init(&kf, 0.0f);  // 初始化卡尔曼滤波器，初始角度0度

    float dt = 0.01f;                  // 采样周期10ms
    float Q_angle = 0.001f;            // 过程噪声协方差（角度）
    float Q_bias = 0.003f;             // 过程噪声协方差（偏置）
    float R_measure = 0.03f;           // 观测噪声协方差

    float gyro = 0.5f;     // 假设陀螺仪测得的角速度（单位：度/秒）
    float accel = 0.4f;    // 假设加速度计计算得到的角度（单位：度）

    for (int i = 0; i < 50; i++) {
        float angle = Kalman_Update(&kf, gyro, accel, dt, Q_angle, Q_bias, R_measure);
        printf("Step %d: Angle = %.4f Bias = %.4f\n", i, angle, kf.bias);
    }

    return 0;
}

1.3.1 这三个参数的含义

1. Q_angle

• 含义：角度的过程噪声协方差

• 作用：描述角度预测模型的不确定性。

• 直白理解：陀螺仪积分出来的角度预测，理想情况下是准确的，但现实中会漂移，会有噪声。这个参数越大，卡尔曼滤波器就会认为预测值更不可信，对观测值（加速度计角度）的依赖会更大。

---

2. Q_bias

• 含义：陀螺仪偏置（bias）的过程噪声协方差

• 作用：描述陀螺仪零偏随时间漂移的不确定性。

• 直白理解：陀螺仪的零偏是个慢慢漂移的量。如果你把 Q_bias 设得很小，滤波器会觉得偏置是稳定的，不太会变。如果设得大，滤波器就会更快地跟踪零偏的变化。

---

3. R_measure

• 含义：观测噪声协方差

• 作用：描述传感器观测值（这里是加速度计计算的角度）的测量噪声大小。

• 直白理解：加速度计算出来的角度在理论上是绝对角度，但实际会抖动、受瞬时线性加速度干扰。R_measure 越大，滤波器会觉得观测值不靠谱，更多依赖预测值（陀螺仪积分）；越小，就更信任观测值。

这里留一个小问题：为什么角速度在使用卡尔曼滤波的时候不需要用角加速度积分来作为预测值？ 

二、互补滤波器

2.1 原理

互补滤波是姿态估计中最简单、最常用的融合方法。它的核心思想就是：高频交给陀螺仪，低频交给加速度计。

• 陀螺仪积分输出响应快，但长期漂移；

• 加速度计输出响应慢，但长期准确；

• 二者做加权平均就能相互弥补，简单稳定。

2.2 互补滤波公式（以角度为例）

2.3 简单示例代码

// 互补滤波函数
// 输入参数：
//   prevAngle  - 上一次计算得到的角度值（度或弧度）
//   gyroRate   - 陀螺仪测量的角速度（单位与dt配合，通常是度/秒或弧度/秒）
//   accelAngle - 由加速度计计算得到的角度（绝对角度）
//   dt         - 时间间隔（秒）
//   alpha      - 滤波系数，范围0~1，权衡陀螺仪和加速度计的信任度
//
// 返回值：
//   计算得到的新角度值（经过滤波）

float Complementary_Filter(float prevAngle, float gyroRate, float accelAngle, float dt, float alpha) {
    // 根据陀螺仪角速度积分得到的角度预测值
    float gyroAngle = prevAngle + gyroRate * dt;

    // 互补滤波，结合陀螺仪积分角度和加速度计角度
    // alpha 越大，越信任陀螺仪；越小，越信任加速度计
    float angle = alpha * gyroAngle + (1 - alpha) * accelAngle;

    return angle;
}

在这里提一下

• 卡尔曼滤波：基于状态空间模型，包含明确的系统模型矩阵（状态转移矩阵 FFF、观测矩阵 HHH 等），用数学方法对系统动态和观测噪声建模，动态调整估计误差协方差 PPP，并通过卡尔曼增益 KKK 自适应权衡预测和观测，实现最优滤波。

• 互补滤波：没有显式的系统矩阵模型，主要是利用经验性加权（滤波系数 α\alphaα）简单地融合陀螺仪积分角度和加速度计角度，依赖权重比例进行滤波，没有状态空间模型和协方差矩阵的更新。（以后更新关于系统模型矩阵的内容）

互补滤波没有系统矩阵模型，而卡尔曼滤波有系统矩阵模型，这也是卡尔曼滤波更精确、更灵活的原因之一。正是因为有系统模型矩阵，卡尔曼滤波才能实现增益的自动调整。互补滤波没有状态空间模型，也没有动态协方差更新，增益(α)是固定的比例，所以没有自动调整的能力。

2.4 为什么角速度不能用互补滤波？

由互补滤波的原理知道：
互补滤波依赖于两个观测值对同一个状态的估计（如：陀螺仪积分角度 vs. 加速度计角度）。
角速度本身并没有第二个观测量可以互补，通常只有陀螺仪角速度，没有可用于矫正的“第二来源”。因此，角速度只能通过单独滤波（如低通滤波、卡尔曼滤波估计零偏），而不能直接用互补滤波。

三、四元数滤波

3.1 原理

对于三维姿态，使用欧拉角会遇到万向节死锁（奇异点）。
四元数是一种无奇异、连续表示旋转的数学工具。
常见的四元数滤波（如 Madgwick、Mahony）会把陀螺仪、加速度计甚至磁力计结合在四元数框架下进行融合。

 3.2 简单代码

// 假设你已经读取了 MPU6050 原始传感器数据：陀螺仪（gx, gy, gz）和加速度计（ax, ay, az）
// gx, gy, gz 单位是弧度/秒，ax, ay, az 是归一化的加速度（重力方向）

float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // 初始四元数，表示无旋转

void MPU6050_QuaternionFilter_Update(float gx, float gy, float gz,
                                     float ax, float ay, float az,
                                     float dt) {
    // 1. 加速度计数据归一化，避免数值漂移
    float norm = sqrtf(ax*ax + ay*ay + az*az);
    if (norm == 0) return; // 防止除零
    ax /= norm; ay /= norm; az /= norm;

    // 2. 通过四元数滤波算法（如Madgwick或Mahony）融合陀螺仪和加速度计数据
    // 这里调用现成的四元数滤波函数，示例为伪函数
    Madgwick_Update(q, gx, gy, gz, ax, ay, az, dt);

    // q数组中保存了融合后的四元数状态，表示当前姿态
}

// 计算欧拉角示例（从四元数转换）
void Quaternion_To_Euler(float q[4], float* roll, float* pitch, float* yaw) {
    *roll  = atan2f(2.0f*(q[0]*q[1] + q[2]*q[3]), 1 - 2*(q[1]*q[1] + q[2]*q[2]));
    *pitch = asinf(2.0f*(q[0]*q[2] - q[3]*q[1]));
    *yaw   = atan2f(2.0f*(q[0]*q[3] + q[1]*q[2]), 1 - 2*(q[2]*q[2] + q[3]*q[3]));
}

3.3 说明与原理解析

• 为什么四元数滤波是对角度进行滤波？
  四元数是用来表示空间的旋转姿态（即角度），它封装了绕三个轴的旋转信息。滤波时，是在四元数空间融合陀螺仪的角速度积分预测和加速度计/磁力计的方向观测，从而得到更准确的角度估计。

• 为什么不能用四元数滤波直接滤角速度？
  四元数本身表示的是旋转状态，不是速度。陀螺仪测量的是角速度，四元数滤波通过积分这些角速度来预测姿态变化，然后通过融合修正。角速度本身是传感器的原始测量，不是状态量的一部分，因此四元数滤波的目标是得到平滑准确的角度，而不是滤波角速度。

• MPU6050 不能直接输出四元数的原因
  MPU6050 本身只输出加速度计和陀螺仪的原始数据（加速度三轴，角速度三轴）。它内部并没有集成复杂的姿态解算算法，也不直接生成四元数。
  只有更高阶的芯片（如 MPU9250 带 DMP 功能，或者更高级的惯性测量单元）才会在内部通过数字运动处理器（DMP）计算四元数并输出。

---

3.4 总结

• MPU6050 采集原始数据后，需要在 MCU 或上位机运行四元数滤波算法（Madgwick、Mahony）完成姿态解算。

• 四元数滤波融合陀螺仪积分预测和加速度计方向观测，滤的是旋转姿态（角度），不直接滤角速度。

• 四元数能避免欧拉角万向锁问题，更适合三维姿态表示和滤波。

• MPU6050 本身无法直接给出四元数，只能提供原始传感器数据，需要外部计算。

 四、 总结

• 卡尔曼滤波：适合低维状态估计（例如角度），需要已知噪声模型；

• 互补滤波：简单高效，适合嵌入式快速实现；

• 四元数滤波：适合三维姿态解算，避免欧拉角奇异问题。

理解原理后，选对方法，才能让 MPU6050 输出的姿态数据更准确、更可靠。