简介：围绕MPU6050与卡尔曼滤波的姿态解算代码包，面向嵌入式开发者、机器人及无人机爱好者，以及需要补强惯性测量单元数据处理能力的学习者，重点解决六轴传感器原始角速度与加速度噪声较大、姿态角估计不稳定等问题。压缩包体积仅2KB，共2个文件，以C源码和头文件为主：源码部分承载滤波器初始化、预测与更新流程，并融合校准后的传感器数据计算俯仰角、翻滚角和偏航角；头文件提供接口声明，便于在其他工程中调用、移植与维护。已有5876人学习，说明姿态融合与滤波落地是项目实践中的高频需求。代码结构精简，突出陀螺仪与加速度计的零点偏移、灵敏度漂移校准思路，以及卡尔曼滤波预测—更新机制在姿态解算中的实现方式，可作为课程设计、飞控或机器人小项目的基础模块，也便于读者理解模块化组织、接口划分和后续扩展方向。

1. 一块六轴模块加一段滤波代码，姿态角到底是怎么稳下来的

四轴起飞那一下抖、云台回中过冲、机械臂末端角度缓慢漂移，排查到最后常常不是电机和机械的问题，而是姿态解算没做干净。MPU6050 卡尔曼滤波这套组合，是嵌入式姿态估计里最常见、也最容易被写歪的一条链路：加速度计给出绝对但毛刺大的角度，陀螺仪给出平滑但会累积漂移的角速度，滤波器的活就是按方差动态分配两者的可信度。

这份资源围绕的正是这条完整链路，从 I2C 读原始六轴数据、零偏校准、量程换算，到四元数更新、协方差整定和卡尔曼增益的每一次迭代。它适合刚拿到六轴模块、串口打出一堆跳变数字不知下一步做什么的人，也适合已经跑通互补滤波、想把解算换成可调协方差版本、需要看清每个矩阵怎么填的开发者。

2. 数据链路先打通：I2C 初始化、量程换算与零偏校准

上板第一步不是写滤波器，而是确认你拿到的六个数字是可信的。很多人跳过这一层直接抄滤波代码，结果调了两天参数，最后发现是量程配错、字节序拼反，这就是典型的把调试时间浪费在黑匣子上。所以这一章先把驱动层做扎实，后面的卡尔曼才有意义。

2.1 分层结构：驱动、解算、应用三层切开

我一般把这类工程切成四层，层与层之间只传物理量，不传寄存器值。这样换主控、换模块型号时，改动面能压到最小。

关键约定是解算层只认国际单位：角速度用 rad/s，加速度用 g 或者 m/s²，角度用 rad。寄存器原始值、LSB 换算全部在驱动层消化掉。这条规矩看着啰嗦，但它是后面所有参数整定能对得上号的前提，单位混着来，Q 和 R 怎么调都是玄学。

2.2 初始化顺序：唤醒、分频、滤波、量程

寄存器写得顺序不对，读出来就是全零或者恒定值。正确顺序是：先解除休眠，等陀螺起振稳定，再配采样率和数字低通，最后配量程。下面这段是移植时只换三个 I2C 函数的版本。

// MPU6050 最小初始化 + 突发读取（Arduino/Wire 风格，换主控只改三件套） #define MPU_ADDR 0x68 // AD0 接地为 0x68，接高电平为 0x69 #define REG_SMPLRT 0x19 // 采样率分频 #define REG_CONFIG 0x1A // 数字低通 DLPF #define REG_GYRO_CFG 0x1B // 陀螺量程 #define REG_ACC_CFG 0x1C // 加速度量程 #define REG_DATA 0x3B // 数据起始寄存器 #define REG_PWR 0x6B // 电源管理 static void mpu_write(uint8_t reg, uint8_t val) { Wire.beginTransmission(MPU_ADDR); Wire.write(reg); Wire.write(val); Wire.endTransmission(); } int mpu_init(void) { mpu_write(REG_PWR, 0x01); // 解除休眠，时钟源选 X 轴陀螺 PLL，比内部 RC 稳 delay(50); // 等陀螺起振，别省这 50ms mpu_write(REG_SMPLRT, 0x04); // 1kHz/(1+4)=200Hz 输出速率 mpu_write(REG_CONFIG, 0x03); // DLPF 约 44Hz，压电机振动 mpu_write(REG_GYRO_CFG, 0x00); // ±250 dps，131 LSB/(deg/s) mpu_write(REG_ACC_CFG, 0x00); // ±2 g，16384 LSB/g return 0; } int mpu_read_raw(int16_t *acc, int16_t *gyro) { Wire.beginTransmission(MPU_ADDR); Wire.write(REG_DATA); if (Wire.endTransmission(false) != 0) return -1; // 地址 NACK，先查接线 if (Wire.requestFrom(MPU_ADDR, 14) != 14) return -2; // 字节数不足，总线被拉低 uint8_t b[14]; for (int i = 0; i < 14; i++) b[i] = Wire.read(); acc[0] = (int16_t)(b[0] << 8 | b[1]); acc[1] = (int16_t)(b[2] << 8 | b[3]); acc[2] = (int16_t)(b[4] << 8 | b[5]); // b[6]、b[7] 是温度，姿态解算可以跳过，做温补时再取 gyro[0] = (int16_t)(b[8] << 8 | b[9]); gyro[1] = (int16_t)(b[10] << 8 | b[11]); gyro[2] = (int16_t)(b[12] << 8 | b[13]); return 0; }

REG_PWR写 0x01 而不是 0x00，差别在时钟源：0x00 用内部 8MHz RC，温漂大；0x01 借用 X 轴陀螺的锁相环，长期稳定性明显好一些。REG_SMPLRT的分频公式是「输出率 = 陀螺采样率 /(1+分频值)」，DLPF 使能时陀螺采样率是 1kHz，所以 0x04 对应 200Hz，正好够 100Hz 的姿态解算用，又不至于让 I2C 总线跑满。REG_CONFIG的 0x03 把带宽压到 44Hz 左右，机架上的高频振动被提前滤掉，这一步做不做，后面卡尔曼的协方差会差一个量级。

mpu_read_raw用一次突发读拿完 14 个字节，中间不做别的操作。分两次读加速度和陀螺会引入时间偏差，高速旋转时这个偏差直接体现成角度误差，属于看不见的坑。返回值 -1 表示从机没应答，-2 表示读到的字节数不够，这两个错误码在排查时要区分开，前者查接线和上拉，后者查总线速率和电源。

2.3 量程换算与静止零偏采集

原始值到物理量的换算系数取决于上一步配的量程。±250 dps 对应 131 LSB/(deg/s)，±2g 对应 16384 LSB/g，这两个数字建议写成常量而不是每次现算，减少运行时除法开销。

// 原始值转物理量：陀螺 deg/s，加速度 g void mpu_convert(const int16_t *acc, const int16_t *gyro, float *acc_g, float *gyro_dps) { for (int i = 0; i < 3; i++) { acc_g[i] = acc[i] / 16384.0f; // ±2g 量程 gyro_dps[i] = gyro[i] / 131.0f; // ±250dps 量程 } } // 上电静止 2 秒采 1000 组求均值，同时检查静止条件是否成立 int mpu_calibrate(float *gyro_bias, float *acc_bias) { const int N = 1000; double sg[3] = {0}, sa[3] = {0}; int16_t a[3], g[3]; int valid = 0; for (int i = 0; i < N; i++) { if (mpu_read_raw(a, g) == 0) { for (int k = 0; k < 3; k++) { sg[k] += g[k]; sa[k] += a[k]; } valid++; } delay(2); } if (valid < N * 9 / 10) return -1; // 丢包超一成，先修总线再来 for (int k = 0; k < 3; k++) { gyro_bias[k] = (float)(sg[k] / valid); // 单位仍是 LSB acc_bias[k] = (float)(sa[k] / valid); } // 静止时合加速度应接近 1g，偏太多说明没放平或者有振动 float amag = sqrtf(powf(acc_bias[0],2) + powf(acc_bias[1],2) + powf(acc_bias[2],2)); if (fabsf(amag - 16384.0f) > 16384.0f * 0.1f) return -2; return 0; }

零偏采集的本质是求均值，但要加两个保护。一是丢包率检查，I2C 接触不良时求出来的均值毫无意义；二是合加速度模长检查，模块放歪或者桌面上有震动，采到的零偏会把误差固化进系统，之后无论怎么调滤波参数都纠不回来。采集期间别去碰模块，连串口打印都建议先关掉，某些板子上串口中断会干扰 I2C 时序。

换算完成后，解算前还要做一步扣零偏：rate = gyro_dps[i] - gyro_bias[i]/131.0f。注意零偏单位，存 LSB 还是存 deg/s 全篇统一，混用一次就够你查一晚上。这一层做完，静止时三轴角速度应该稳定在 ±0.3 dps 以内，超出这个范围就先别往下走，滤波器救不了脏数据。

3. 姿态解算骨架：四元数更新、欧拉角输出与坐标系约定

驱动层给出干净的角速度和加速度，接下来要把角速度积分成姿态。用欧拉角直接积分会在俯仰角接近 90 度时出现万向节死锁，用旋转矩阵要维护九个元素还得反复正交化，四元数四个元素、只需归一化，是工程上最省事的选择。这一章把坐标系、更新方程和欧拉角转换一次讲透，后面卡尔曼才有落脚点。

3.1 坐标系与旋转顺序先定死

姿态解算里一半的「算出来方向反了」都源于坐标系没统一。我的习惯是：机体坐标系 X 轴指向正前方，Y 轴指向右，Z 轴指向下，构成右手系；世界坐标系固定为东北天，重力方向沿世界系 Z 轴负方向。旋转顺序按 Z-Y-X 做，也就是先偏航、再俯仰、最后横滚。

四元数定义为 q = [w, x, y, z]，对应绕单位轴旋转。写代码前先在纸上画一遍你的模块贴装方向：芯片丝印朝上还是朝下、X 轴箭头指向机头还是机尾，这个方向搞错，滤波收敛得再漂亮，输出也是反的。我见过不止一次有人用软件去补偿一个本来该在装配阶段解决的问题，越补越乱。

3.2 四元数微分方程的离散更新

角速度到四元数的一阶离散更新，本质是把旋转增量乘到当前姿态上。下面这段每毫秒到十毫秒调一次，dt 用实际间隔而不是标称值。

// q = [w, x, y, z]，角速度单位 rad/s，dt 单位秒 void quat_update(float *q, const float *w, float dt) { float hx = w[0] * 0.5f * dt; float hy = w[1] * 0.5f * dt; float hz = w[2] * 0.5f * dt; float qw = q[0], qx = q[1], qy = q[2], qz = q[3]; q[0] += (-qx * hx - qy * hy - qz * hz); q[1] += ( qw * hx + qy * hz - qz * hy); q[2] += ( qw * hy - qx * hz + qz * hx); q[3] += ( qw * hz + qx * hy - qy * hx); float n = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]); q[0] /= n; q[1] /= n; q[2] /= n; q[3] /= n; // 每步归一化，别攒着 }

四个更新项里，qw那一行是标量部分的变化，后面三行对应三个虚部。系数 0.5 来自四元数微分方程 q' = 0.5 * q ⊗ ω 的推导。dt 千万不要用固定常数，主控的定时器有抖动，I2C 读取也会引入随机延迟，建议用两次调用之间的实际微秒差换算。归一化必须每步都做，攒到几十步再归一化，模长发散后姿态会出现莫名其妙的角度跳变，这是新手最容易翻的车。

角速度进来之前记得把单位从 deg/s 转成 rad/s，乘 π/180。漏掉这一步，姿态会以 57 倍的速率疯转，现象特别吓人，原因却很低级。

3.3 从四元数到欧拉角，以及俯仰角附近的边界处理

上层控制环多数还是按欧拉角写 PID，所以要有转换接口。转换公式本身不难，难在俯仰角接近 ±90 度时 asin 的输入会略微超过 1，产生 NaN 并沿着控制链路一路污染下去。

// 四元数转欧拉角，输出顺序 roll、pitch、yaw，单位 rad void quat_to_euler(const float *q, float *rpy) { float w = q[0], x = q[1], y = q[2], z = q[3]; rpy[0] = atan2f(2.0f * (w * x + y * z), 1.0f - 2.0f * (x * x + y * y)); // roll float s = 2.0f * (w * y - z * x); if (s > 1.0f) s = 1.0f; // 限幅，防 NaN if (s < -1.0f) s = -1.0f; rpy[1] = asinf(s); // pitch rpy[2] = atan2f(2.0f * (w * z + x * y), 1.0f - 2.0f * (y * y + z * z)); // yaw }

atan2f的参数顺序两个分量别写反，反了之后 roll 和 yaw 会在某些象限互换，现象是倾斜时角度对、转一圈后角度错。asinf前的限幅是必须的，浮点误差让输入变成 1.0000001 就返回 NaN，而 NaN 参与后续滤波会永久污染协方差矩阵，属于典型的「一次污染、终身带病」。限幅之后，俯仰角在 ±90 度附近会有一段非线性区，这是欧拉角表示法本身的缺陷，不是代码问题，控制环在极限姿态下要单独处理。

yaw 这块要提前说清楚：只有六轴、没有磁力计，yaw 是不可观测的，只能靠陀螺积分，必然缓慢漂移。滤波能改善 roll 和 pitch，对 yaw 无能为力，后面对卡尔曼的期望值要建立在这个认知上。

4. 卡尔曼滤波落地：从一维双状态到六维误差状态

互补滤波一行代码就能跑，为什么还要上卡尔曼？差别在自适应：滤波器维护协方差，能在剧烈运动和静止之间自动切换对陀螺和加速度计的信任度。代价是要填 Q 和 R，填不对的时候效果确实可能不如互补滤波。这一章先给能直接抄的一维双状态实现，再往上走到误差状态版本。

4.1 一维双状态：角度和零偏一起估

工程上最常见的做法是把 roll 和 pitch 拆成两个独立的一维滤波器，状态取 [角度, 陀螺零偏]。这个模型简单，但能把零偏在线估出来，长期漂移改善明显。

typedef struct { float angle; // 角度估计，rad float bias; // 陀螺零偏估计，rad/s float P[2][2]; // 协方差矩阵 float Q_angle; // 角度过程噪声 float Q_bias; // 零偏过程噪声 float R_meas; // 加速度观测噪声 } KF1D; // gyro_rate：扣过静态零偏的角速度 rad/s // acc_angle：加速度解算出的角度 rad float kf1d_update(KF1D *kf, float gyro_rate, float acc_angle, float dt) { // 预测：用陀螺积分推进角度 float rate = gyro_rate - kf->bias; kf->angle += rate * dt; // 协方差预测 P = F*P*F' + Q kf->P[0][0] += dt * (dt * kf->P[1][1] - kf->P[0][1] - kf->P[1][0] + kf->Q_angle); kf->P[0][1] -= dt * kf->P[1][1]; kf->P[1][0] -= dt * kf->P[1][1]; kf->P[1][1] += kf->Q_bias * dt; // 更新：加速度角度作为观测 float S = kf->P[0][0] + kf->R_meas; // 新息协方差 float K0 = kf->P[0][0] / S; // 角度增益 float K1 = kf->P[1][0] / S; // 零偏增益 float y = acc_angle - kf->angle; // 新息 kf->angle += K0 * y; kf->bias += K1 * y; // 协方差更新，用 Joseph 形式的简化写法 float P00 = kf->P[0][0], P01 = kf->P[0][1]; kf->P[0][0] -= K0 * P00; kf->P[0][1] -= K0 * P01; kf->P[1][0] -= K1 * P00; kf->P[1][1] -= K1 * P01; return kf->angle; }

结构体里五个参数，作用完全不同。Q_angle描述「角度模型有多不可信」，取值通常在 1e-4 到 1e-2 量级；Q_bias描述零偏的变化速度，温漂慢，一般给 1e-6 到 1e-4；R_meas描述加速度计算角度的噪声，静止时小、振动时大，典型值 0.01 到 0.5。这三个值不是拍脑袋定的，后面第 6 章会讲怎么用静态数据反推。

S是新息协方差，K0和K1分别是角度和零偏的卡尔曼增益。注意K1来自 P[1][0]，它让零偏估计能跟着观测误差慢慢修正，这是双状态模型相对单状态的全部价值。协方差更新里我用的是简化写法，不是严格的 Joseph 形式，因为这里观测矩阵 H = [1, 0]，简化版在数值上足够稳定。

加速度算角度用atan2f(ay, az)和atan2f(-ax, sqrt(ay²+az²))，两个公式分别对应 roll 和 pitch。写的时候注意符号，不同贴装方向符号可能相反，用静态倾斜测试对一遍，倾角 30 度时输出应该也是 30 度。

4.2 Q 和 R 的物理含义与初始值

这三个参数决定了滤波器的性格：Q 大表示信陀螺少、更依赖加速度，响应快但抖；R 大表示信加速度少、更依赖陀螺，平滑但滞后。调试顺序我习惯先定 R，再调 Q_angle，最后动 Q_bias。

R 的估计有据可依：模块静止放在桌面上采几分钟，用加速度算出的角度序列求方差，那个值就是 R 的下界。振动环境下实际 R 会大得多，所以在电机转起来之后再采一组，取两者之间偏保守的值。Q_angle 一般从 1e-3 起步，观察阶跃响应，超调明显就调小，跟不上就调大。Q_bias 影响的是长期漂移的收敛速度，给太小零偏估不动，给太大静止时角度会自己慢慢爬。

提示：改参数前先把原始角速度和加速度用串口或者 SD 卡录一段，离线回放调参，比在板子上反复烧录快十倍不止。

4.3 误差状态 EKF：把三轴耦合一起处理

一维滤波器把 roll 和 pitch 拆开，忽略了轴间耦合，大角度机动时误差会上来。要做完整解，状态取误差四元数 δq 和陀螺零偏 δb，共六维，用四元数做名义状态递推，误差状态做卡尔曼更新。

// 简化版误差状态更新：名义四元数由陀螺积分，观测量为归一化重力方向 // q：当前四元数，b：零偏估计，acc_n：归一化加速度，R：观测噪声标量 void ekf_update(float *q, const float *b, const float *acc_n, float R) { float qw = q[0], qx = q[1], qy = q[2], qz = q[3]; // 机体系下的重力方向预测（旋转矩阵第三列的反向） float h[3] = { 2.0f * (qx * qz - qw * qy), 2.0f * (qw * qx + qy * qz), qw * qw - qx * qx - qy * qy + qz * qz }; // 残差用叉乘构造，避免直接相减引入非线性 float r[3] = { acc_n[1] * h[2] - acc_n[2] * h[1], acc_n[2] * h[0] - acc_n[0] * h[2], acc_n[0] * h[1] - acc_n[1] * h[0] }; // 观测矩阵对误差四元数虚部的偏导，做小角度近似 float H[3][3] = { { 0.0f, h[2], -h[1] }, { -h[2], 0.0f, h[0] }, { h[1], -h[0], 0.0f } }; // 标量近似增益：忽略轴间相关，工程上够用且省算力 float K = 1.0f / (1.0f + R); float dq[3]; for (int i = 0; i < 3; i++) { dq[i] = K * (H[i][0] * r[0] + H[i][1] * r[1] + H[i][2] * r[2]); } // 误差四元数转回名义四元数并归一化 q[0] += -0.5f * ( q[1] * dq[0] + q[2] * dq[1] + q[3] * dq[2]); q[1] += 0.5f * ( q[0] * dq[0] + q[2] * dq[2] - q[3] * dq[1]); q[2] += 0.5f * ( q[0] * dq[1] - q[1] * dq[2] + q[3] * dq[0]); q[3] += 0.5f * ( q[0] * dq[2] + q[1] * dq[1] - q[2] * dq[0]); float n = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]); q[0] /= n; q[1] /= n; q[2] /= n; q[3] /= n; }

h是预测的机体系重力方向，由四元数直接算出，acc_n是实测归一化重力方向。残差用叉乘而不是直接相减，是因为两个单位向量的叉乘在小角度下近似等于角度误差向量，线性度更好。H矩阵是观测方程对误差四元数虚部的偏导，小角度近似下就是h的反对称矩阵，这一步推导在多数教材里都能找到，照着填即可。

K这里用了标量近似，把三轴当成独立通道处理，省掉了 3×3 求逆。真正的完整实现要维护 6×6 的协方差矩阵，做P Hᵀ (H P Hᵀ + R)⁻¹，主控算力够、姿态精度要求高的场合再上。移植到 Cortex-M0 这类没有 FPU 的芯片上，完整 EKF 的浮点开销要提前评估，实在跑不动就退回一维双状态版本，工程上并不丢人。

观测前必须把加速度归一化，模长扔掉。这一步很关键，加速度计测的是比力，机动时会叠加线加速度，归一化相当于只取方向信息，把模长扰动的影响削弱一部分。更严谨的做法是根据模长偏离 1g 的程度动态放大 R，模长越离谱越不信这次观测，实现上就一行判断，效果立竿见影。

5. 避坑与排查：姿态跳变、漂移、越界的五个真实原因

滤波器跑起来不难，跑稳很难。下面这几条是调这类项目时反复出现的现象，每条按现象、原因、解决三段说，遇到了可以直接对号入座。

5.1 静止时角度纹丝不动，一推就大幅跳变

现象是模块放桌上输出稳定在某个值，手一推，角度瞬间跳十几度然后慢慢回落。原因多半是 DLPF 没配或者配成了最宽带宽，加速度计把高频噪声全带进来了，卡尔曼的 R 又设得偏小，滤波器对每一次噪声观测都过度反应。

解决办法是把REG_CONFIG设成 0x03 或 0x04，带宽压到 44Hz 或 21Hz，同时把 R 调大一个量级，从 0.01 往上试到 0.1。改完再看阶跃响应，如果响应变慢但不再跳变，说明方向对了。另一个隐蔽原因是陀螺量程选了 ±2000 dps，分辨率只有 16.4 LSB/(deg/s)，静止时的量化噪声被放大，能换 ±250 就换。

5.2 上电十分钟后角度缓慢漂移

现象是刚上电零偏校准完很准，运行一段时间后静止角度慢慢偏，十几分钟能偏好几度。原因是零偏随温度变化，芯片自己发热加上环境温度，静止时采的那组零偏早就不适用了。

光靠开机校准治不了这个，得让滤波器在线估零偏，也就是第 4 章双状态模型里那个bias状态。如果已经在用双状态还是一直漂，检查Q_bias是不是给得太小，零偏状态被冻住了。另一个检查点是dt的真实性，用标称的 5ms 而实际间隔是 8ms，积分出来的角度会系统性偏快，表现也像漂移，用示波器或者翻转 GPIO 量一下实际周期就能排除。

5.3 偏航角自己转个不停

现象是模块完全静止，roll 和 pitch 稳定，yaw 却以每分钟几度的速度匀速转。这不是 bug，是六轴系统的固有限制：没有磁力计，yaw 方向没有绝对参考，只能靠陀螺积分，零偏残余乘以时间就是角度漂移。

能做的有三件事。一是把静止零偏校得更准，采集时间拉长到 5 秒以上，并且等芯片温度稳定后再采。二是把 yaw 的陀螺零偏也纳入在线估计，逻辑和 roll、pitch 一样。三是接受它，在使用侧做处理：需要绝对航向就加磁力计或者外部参考，不需要就别让 yaw 参与闭环。指望纯六轴卡尔曼把 yaw 稳住，属于对物理限制缺乏认知。

5.4 电机一转，数据全乱

现象是地面静止调试一切正常，电机一启动，姿态输出就开始剧烈抖动甚至发散。原因是振动通过机架传到模块上，加速度计被高频振动淹没，同时 I2C 走线在强电磁环境下受干扰，出现丢包和误码。

解决要分两头。机械上做减震，双面胶加配重或者减震球，把模块和机架的刚性连接断开。电气上缩短 I2C 走线，上拉电阻从 4.7k 降到 2.2k 增强边沿，电源加 LC 滤波或者单独一路 LDO。软件上把 DLPF 压到 21Hz，读取函数检查返回值，连续 NACK 就重新初始化总线而不是硬读。三管齐下之后，多数机架的振动问题能压到可接受范围。

5.5 移植到新主控后输出 NaN 或者恒定值

现象是同一份解算代码在原来的板子上正常，换到新主控后欧拉角一直是 NaN，或者卡在 0 不动。原因是新平台的float运算、atan2f实现或者字节序有差异，也可能是 I2C 读出来的原始值拼装顺序反了。

排查按顺序来：先把原始六个 int16 打出来，静止时应该接近零偏值，如果数值巨大或者正负乱跳，问题在 I2C 读；再打印换算后的物理量，重力三轴合模长应该在 1g 附近，不对说明量程系数配错；最后打印四元数模长，应该恒为 1，偏离说明归一化没生效。atan2f在部分精简数学库里精度较差，俯仰角接近 90 度时 asin 输入超出 [-1,1] 需要限幅，这一条在第 3 章已经写过，移植时最容易漏。

6. 二次开发：协方差整定、离线回放与移植验证

真正决定这套代码好不好用的，不是滤波公式写得多漂亮，而是参数整定和验证流程是否可复现。我一般先把原始数据录下来，再离线跑滤波，避免在板子上反复烧录。录制时按固定周期把六轴原始值和时间戳一起写进数组或者 SD 卡，采样率 200Hz，录 30 秒静止、30 秒慢速摆动、10 秒剧烈晃动，这三段数据基本覆盖了日常工况。

离线回放用 Python 就够，把 C 里的滤波逻辑照着写一遍，用 matplotlib 把原始角度、滤波输出和陀螺积分三条曲线画在同一张图上，参数改动立刻能看到效果。回放脚本里注意时间戳要用录制时的真实间隔，不要用固定步长，否则整定出来的参数上板会偏。

移植到新平台时，我习惯按「原始值、物理量、四元数、欧拉角」四级逐级打点验证，每一级对上了再往下一级走。这四级里任何一级出问题，后面的结果都没有意义，跳过验证直接看最终角度，等于把排查成本放大十倍。I2C 读取的返回值一定要检查，不要为了省事忽略错误码，总线偶发 NACK 被当成有效数据送进滤波器，协方差会瞬间被带偏，之后的收敛全靠运气。

最后一条血泪经验：每次改滤波参数之前，先把当前能跑的版本备份一份，并在代码里写清楚这组参数对应什么工况。参数调坏是常事，没有后悔药的时候只能靠记忆复原，而记忆在连续调了三小时之后基本不可信。从那以后我每次动 Q 和 R 之前都强制存一份带注释的参数快照，跑完对比再决定留哪个。希望这些能帮你在自己的板子上少绕几圈。

本文还有配套的精品资源，点击获取