简介：面向嵌入式与惯性导航初学者及硬件爱好者的MPU6050姿态解算代码包，聚焦用六轴IMU原始数据融合出俯仰、横滚与偏航角的实际问题，适合作为无人机、机器人及运动检测项目的姿态估计参考。压缩包共2个文件，以1个C源文件和1个头文件构成，整体约2KB，体量轻巧，核心逻辑集中在卡尔曼滤波初始化、预测与更新流程，并通过头文件对外声明调用接口，便于嵌入现有工程或按模块阅读。代码围绕MPU6050初始化、寄存器原始角速度与加速度读取、传感器零点偏移和灵敏度漂移校准展开，再将校准数据送入滤波器完成姿态角计算，能帮助读者理解从噪声数据到稳定姿态输出的完整链路。目前已有5876人学习，适合需要快速搭建滤波框架、对照校准思路与模块化组织方式的学习者。

1. 姿态解算的第一道坎：原始数据不处理，卡尔曼滤波也救不回来

去年帮人调一个自平衡的两轮小车，车一站上去就往一侧倒，现象很怪：静止时角度读数每隔十几秒会缓慢偏移三四度，一动起来又抖得厉害。很多人第一反应是「换个更好的滤波算法」，但我把日志打出来一看，陀螺仪静置时的零偏根本没标定，dt还是写死的0.01，实际采样周期在 12～18ms 之间来回变化，卡尔曼滤波再精确，喂给它的输入就是错的，输出自然就是错误的。

这也是我写这份 MPU6050 卡尔曼滤波姿态解算资料的原因。它面向的是手上已经有一块六轴模块、想真正把 roll / pitch 算准而不是「能动就行」的开发者：从 I2C 寄存器配置、零偏标定，到单轴卡尔曼滤波的预测与更新两段代码，再到双轴封装和上位机联调，一条链路走完。读完你至少能判断，项目里那些漂移和抖动，到底该怪传感器、怪标定，还是怪滤波参数。

2. 硬件与数据链路：MPU6050 输出的到底是什么

2.1 I2C 时序、寄存器映射与量程选择

MPU6050 是一颗六轴惯性器件，三轴加速度计加三轴陀螺仪，通过 I2C 挂在主控上。AD0 引脚接低时从机地址是0x68，接高是0x69，很多模块板上已经把这个引脚下拉了，所以默认扫到0x68。第一次上手最容易卡住的地方不是算法，而是「读回来全是 0」——电源管理寄存器0x6B上电默认是0x40，芯片处于睡眠态，不写它就不会出数据。

数据结构上，从0x3B开始连续 14 个字节是加速度三轴、温度、陀螺三轴，全部是 16 位大端有符号数。这个「连续」很重要：如果分成两次读，一次读加速度、一次读陀螺，中间隔了几百微秒，两个物理量就不在同一时刻，动态下算出来的角度会有系统性偏差。经验做法是一次突发读完整块，再在本地拆。

量程怎么挑，取决于你的载体。云台、机械臂这类慢速机构，陀螺用 ±250°/s 就够，分辨率更高；两轮车、四轴这类快速姿态变化，±500 甚至 ±1000 更安全，因为量程一旦被顶到饱和，输出会被钳在极值，滤波会把这个错误当成真实角速度积分进去，姿态瞬间出错，而且不会自己恢复。加速度计同理，手持设备和慢速平台 ±2g 足够，有跌落冲击或者电机振动传导的场景，宁可放到 ±8g，牺牲分辨率换不饱和。

时钟源这一项很多人忽略。0x6B写0x00也能跑，但用的是内部 8MHz RC 振荡器，温漂大，陀螺积分会跟着温漂走。写0x01切到 X 轴陀螺 PLL，稳定性明显好一截。这类细节不属于算法范畴，但它决定了你后面调参时的基准是否可复现。

2.2 六轴物理量与姿态可观性：为什么先做单轴

加速度计测的是比力，静止时输出就是重力在三个轴上的投影。所以静止或匀速状态下，可以直接反解出倾角：

• roll = atan2(ay, az)
• pitch = atan2(-ax, sqrt(ay² + az²))

这两个公式在 ±90° 范围内都成立，pitch 用 sqrt 而不是直接 atan2(ax, az)，就是为了避免 roll 接近 ±90° 时欧拉角奇异带来的耦合误差。

陀螺仪测的是角速度，积分得到角度，短时间极其平滑，但零偏会被时间放大——1°/s 的零偏，一分钟就是 60° 的累积误差。加速度计反过来，长期不漂，但任何线性加速度（走动、电机振动、车体加减速）都会被误当成重力分量，噪声很大。

这两者的互补性就是所有姿态融合算法的出发点。卡尔曼滤波在这里干的事情，本质是「用陀螺的短时可信度，去平滑加速度计的噪声；用加速度计的长期无漂，去校正陀螺的零偏」。

有一个结论必须提前说清楚：只用六轴，yaw（航向角）是不可观测的。加速度计感受不到绕重力轴的旋转，陀螺积分出来的 yaw 没有绝对参考，一定漂。所以单靠 MPU6050 做不出绝对航向，要么加磁力计做九轴，要么接受 yaw 只能做相对增量。这一点在选型阶段就要认，不然后面会白折腾。

也正因为 yaw 不可观测，工程上更稳妥的做法是先把 roll 和 pitch 两个轴做扎实。两个轴的物理模型完全独立、公式对称，只有一个轴跑通了，复制到另一轴才不会有隐藏问题。我一般建议新手先只做 roll，用串口把滤波前后的角度同时打出来对比，肉眼能看出滤波到底在做什么，比一上来就双轴加四元数直观得多。

2.3 采样节拍与时间基准：dt 不是常数

滤波效果好不好，一半取决于dt准不准。卡尔曼滤波的预测步是angle += (gyro - bias) * dt，这里的dt是两次采样的真实时间间隔。如果主循环里写delay(10)然后假定dt = 0.01，实际因为 I2C 通信、串口打印、其他任务的干扰，真实间隔可能在 10～20ms 之间抖动。误差 100% 的dt会让角度积分直接错一倍，表现出来就是「静止还行，一转就偏」。

正确做法是用硬件定时器或者系统滴答计数器打时间戳，两次采样做差得到真实dt，并且给一个合理区间做保护：

/* 用单调递增的微秒计时器取真实采样间隔 */ static uint32_t last_us = 0; float get_dt(void) { uint32_t now = micros(); float dt = (now - last_us) * 1e-6f; /* 换算成秒 */ last_us = now; if (dt <= 0.0f || dt > 0.1f) /* 首次采样或明显异常，退回标称周期 */ dt = 0.01f; return dt; }

micros()换成你自己的平台对应的微秒计时函数即可。上限0.1f是保护：调试时打断点、或者串口阻塞导致停了半秒，这个dt如果直接拿去积分，角度会被一次性放大几百度，滤波器要花很久才收敛回来，甚至直接发散。宁可丢掉这一帧，也不要喂一个离谱的dt。

采样率本身建议 100～200Hz。太低（比如 20Hz），陀螺积分在快速转动时误差明显，滤波跟不上；太高，I2C 带宽和 CPU 都被占满，收益却很小——MPU6050 的 DLPF 已经把带宽压到 44Hz 了，采再快也只是重复噪声。100Hz 配 44Hz 低通，是这个器件上一个比较舒服的平衡点。

3. 工程骨架：从寄存器到可运行姿态解算的分层

3.1 目录结构与模块职责

这份资料的代码组织方式是按「硬件相关」和「算法相关」彻底切开。这样做的直接好处是：换主控只改bsp层，换滤波算法只改filter层，两边互不影响。实际项目里我踩过的坑就是把 I2C 读写和卡尔曼滤波写在一个文件里，后来要换平台，改一处牵连一片。

mpu6050_attitude/ ├── bsp/ │ ├── i2c_port.c # I2C 收发封装，平台相关 │ └── time_port.c # 微秒计时、毫秒延时 ├── driver/ │ ├── mpu6050.c # 寄存器配置、原始数据读取 │ └── mpu6050.h ├── filter/ │ ├── kalman1d.c # 单轴卡尔曼，无任何硬件依赖 │ └── kalman1d.h ├── app/ │ ├── attitude.c # 双轴封装、角度输出 │ └── main.c # 采样节拍与任务调度 └── tools/ └── plot.py # 上位机串口绘图与数据分析

分层的界线在driver和filter之间。driver只负责把 14 个字节变成物理量（单位 g 和 °/s），不关心这些数用来干什么；filter只接收「角速度 + 观测角度 + dt」，不知道数据来自哪颗芯片，也不知道主控是什么。这个边界划清楚之后，filter层的代码可以直接拿到 PC 上跑仿真：录一段真实数据存成 CSV，离线灌进去调参，比在板子上反复烧录快十倍。

tools/plot.py不是可有可无的附属品。姿态调参这件事，靠盯着串口数字看是调不出来的，必须把陀螺积分角、加速度角、滤波输出三条曲线叠在一张图上，才能看出谁在漂、谁在抖、滤波器跟得快不快。这一层我在每个姿态项目里都会加上。

3.2 初始化与零偏标定：这一步省不掉

初始化负责三件事：确认芯片在位、配置量程与滤波、采集静止样本标定陀螺零偏。

/* 上电初始化：先验证在位，再逐项配置，最后标定零偏 */ int mpu6050_init(i2c_bus_t *bus, uint8_t addr, mpu6050_t *dev) { uint8_t who = 0; i2c_read_reg(bus, addr, 0x75, &who, 1); if (who != 0x68) /* 地址错、虚焊、供电异常都在这里暴露 */ return -1; i2c_write_reg(bus, addr, 0x6B, 0x80); /* 软复位 */ delay_ms(100); i2c_write_reg(bus, addr, 0x6B, 0x01); /* 唤醒 + 陀螺 PLL 时钟源 */ i2c_write_reg(bus, addr, 0x19, 0x09); /* 100Hz 采样 */ i2c_write_reg(bus, addr, 0x1A, 0x03); /* DLPF 44Hz */ i2c_write_reg(bus, addr, 0x1B, 0x08); /* 陀螺 ±500°/s */ i2c_write_reg(bus, addr, 0x1C, 0x10); /* 加速度 ±8g */ return mpu6050_calibrate(bus, addr, dev, 1000); }

mpu6050_calibrate的内容是：保持模块完全静止，连续采 1000 组陀螺数据取平均，得到的均值就是零偏，存进dev->gyro_bias[3]，后续每次读取都减掉它。1000 组在 100Hz 下是 10 秒，听起来久，但这是唯一一次，值得等。

这里有两个必须注意的点。第一，标定期间模块必须真的静止，放在桌面上、电机不转、风扇不吹，任何持续振动都会把振动分量算进零偏里。第二，零偏随温度变化，冷机标定的值拿到热机状态用，会残留几十毫度每秒的偏差。低成本做法是在每次上电时都重新标定一次，并且要求设备上电后静止几秒；讲究一点的做法是用片上温度传感器做一阶补偿，0x41那两个字节就是温度，能读到 0.1℃ 分辨率。

/* 零偏标定：静止采样求均值，后续每次读数都减掉 */ int mpu6050_calibrate(i2c_bus_t *bus, uint8_t addr, mpu6050_t *dev, int n) { float sum[3] = {0}; imu_sample_t s; for (int i = 0; i < n; i++) { if (mpu6050_read(bus, addr, &s) != 0) return -2; for (int a = 0; a < 3; a++) sum[a] += s.gyro[a]; delay_ms(10); /* 与 100Hz 采样节拍对齐 */ } for (int a = 0; a < 3; a++) dev->gyro_bias[a] = sum[a] / n; /* 单位 °/s，不是原始 LSB */ return 0; }

参数n取 1000 是经验值：太少（比如 100 组）均值受噪声影响大，标定结果本身就有 ±0.2°/s 的随机误差；太多收益递减，还要用户多等。注意这里存的是换算成 °/s 之后的偏置，不是原始 LSB 值，这样滤波层拿到的数据单位统一，不会在单位换算上出错——单位混用是姿态代码里最常见的隐蔽 bug，而且出错时现象是「角度慢慢偏」，特别像算法问题。

3.3 采集与解算主循环：数据流怎么串

主循环的逻辑很直白：按节拍取一帧数据，算dt，算出加速度观测角，喂给滤波器，拿到滤波后的角度输出。

void attitude_task(attitude_t *att) { imu_sample_t s; if (mpu6050_read(att->bus, att->addr, &s) != 0) return; /* 读失败直接跳过本帧，不要用旧数据 */ float dt = get_dt(); if (dt < 0.002f) return; /* 间隔过短，重复帧，丢掉 */ for (int a = 0; a < 3; a++) s.gyro[a] -= att->dev.gyro_bias[a]; /* 先补零偏，再进滤波 */ /* 加速度反解倾角，作为卡尔曼的观测输入 */ float acc_roll = atan2f(s.acc[1], s.acc[2]) * RAD2DEG; float acc_pitch = atan2f(-s.acc[0], sqrtf(s.acc[1] * s.acc[1] + s.acc[2] * s.acc[2])) * RAD2DEG; att->roll = kalman1d_update(&att->k_roll, s.gyro[0], acc_roll, dt); att->pitch = kalman1d_update(&att->k_pitch, s.gyro[1], acc_pitch, dt); }

几个参数和写法值得说明。RAD2DEG是 57.29578，全流程统一用度，避免有人用弧度有人用度。s.gyro[a] -= bias放在滤波之前，是因为滤波器内部还会自己估计一个零偏量，这两者不冲突：外部标定消除的是确定的静态偏置，滤波器内部的bias状态跟踪的是残余的、随时间缓慢变化的偏置。

dt < 0.002f这一句是防重复帧。有时上层调度器抖动会让两帧间隔只有几百微秒，这么小的dt会让协方差矩阵更新失去数值意义，表现出来是角度在某些时刻突然跳一下。丢掉比处理更省事。

mpu6050_read返回非 0 时直接return，不复用上一帧数据。这一点看起来是细节，实际上很关键：I2C 偶发 NACK 时如果用旧数据，等于给滤波器喂了一帧「时间前进了但数据没变」的假信息，陀螺积分会凭空多走一段，角度出现台阶。宁可丢帧，不要假帧。

4. 核心算法拆解：单轴卡尔曼滤波的落地写法

4.1 状态方程与观测方程怎么定

姿态解算用的卡尔曼滤波不需要搞成完整的多维形式，单轴两状态就够了，这也是工程上最稳的做法。状态向量取：

x = [angle, bias]ᵀ

angle是这一轴的角度，bias是陀螺的残余零偏。选两个状态而不是一个，是因为单状态滤波器没法在线估计零偏，标定残差会一直累积；把 bias 也作为状态，滤波器就能在运行过程中自己把残余偏置慢慢吃掉。

状态转移（预测）写成离散形式：

• angle(k) = angle(k-1) + (gyro_rate - bias(k-1)) * dt
• bias(k) = bias(k-1)

对应的雅可比矩阵 F = [[1, -dt], [0, 1]]。观测方程只有角度是可测的：

• z(k) = angle(k) + v，观测矩阵 H = [1, 0]

于是整个过程就是教科书上的两段：预测用陀螺推进状态和协方差，更新用加速度观测修正。

过程噪声协方差 Q 只有两个可调量，q_angle和q_bias；观测噪声 R 是一个标量r_measure。三个参数，对应三个物理含义，比很多人想象的简单。

要强调一点：这个模型假设bias是缓慢变化的常量，所以bias的过程噪声q_bias应该设得很小。如果q_bias设大了，滤波器会认为零偏可以剧烈变化，于是把加速度计的噪声也当成零偏去跟踪，结果是角度跟着加速度噪声一起抖。这是调参时最容易犯的错误。

4.2 预测与更新两段代码逐段讲

typedef struct { float q_angle; /* 角度过程噪声，越大越信陀螺 */ float q_bias; /* 零偏过程噪声，通常很小 */ float r_measure; /* 观测噪声，越大越不信加速度计 */ float angle; /* 状态：角度，单位度 */ float bias; /* 状态：陀螺零偏，单位 °/s */ float P[2][2]; /* 状态协方差矩阵 */ } kalman1d_t; void kalman1d_init(kalman1d_t *k, float q_angle, float q_bias, float r_measure) { k->q_angle = q_angle; k->q_bias = q_bias; k->r_measure = r_measure; k->angle = 0.0f; k->bias = 0.0f; k->P[0][0] = 1.0f; /* 初始协方差别设 0，否则滤波器永远学不进去 */ k->P[0][1] = 0.0f; k->P[1][0] = 0.0f; k->P[1][1] = 0.1f; } float kalman1d_update(kalman1d_t *k, float gyro_rate, float acc_angle, float dt) { /* ---------- 预测：陀螺积分推进角度 ---------- */ float rate = gyro_rate - k->bias; k->angle += dt * rate; /* P = F·P·Fᵀ + Q，F = [[1,-dt],[0,1]]，展开后就是下面四行 */ k->P[0][0] += dt * (dt * k->P[1][1] - k->P[0][1] - k->P[1][0] + k->q_angle); k->P[0][1] -= dt * k->P[1][1]; k->P[1][0] -= dt * k->P[1][1]; k->P[1][1] += k->q_bias * dt; /* ---------- 更新：加速度角度作为观测 ---------- */ float s = k->P[0][0] + k->r_measure; /* 新息协方差 S = H·P·Hᵀ + R */ float K0 = k->P[0][0] / s; /* 角度增益 */ float K1 = k->P[1][0] / s; /* 零偏增益 */ float y = acc_angle - k->angle; /* 新息：观测与预测之差 */ k->angle += K0 * y; k->bias += K1 * y; /* P = (I - K·H)·P，H = [1,0]，只用前两行参与运算 */ float P00 = k->P[0][0], P01 = k->P[0][1]; k->P[0][0] -= K0 * P00; k->P[0][1] -= K0 * P01; k->P[1][0] -= K1 * P00; k->P[1][1] -= K1 * P01; return k->angle; }

先看预测步。rate = gyro_rate - k->bias是扣掉估计零偏后的真实角速度，乘dt累加到角度上，这是滤波器里唯一真正「积分」的地方。后面四行协方差更新看着复杂，其实就是F·P·Fᵀ + Q的展开，展开比调用矩阵库快得多，在单片机上这点开销差别很明显——100Hz 下每秒要做 200 次，用通用矩阵乘法会多出几十微秒。

k->P[1][1] += k->q_bias * dt这一句决定零偏估计的跟随速度。q_bias越大，协方差增长越快，滤波器越愿意相信「零偏变了」。

再看更新步。s = P[0][0] + r_measure是新息协方差，K0就是角度增益。注意K0 = P[0][0] / s这个式子：P[0][0]是当前对角度估计的不确定度，r_measure是观测的不确定度。当角度估计很自信（P[0][0]小），K0就小，滤波器基本不听加速度计的；当角度积分久了不确定度涨上来，K0变大，就更多地采纳加速度计的观测。这个自适应过程是卡尔曼相对于固定权重的互补滤波的核心优势。

y = acc_angle - k->angle是横滚新息量，两种情况下它会异常大：一是加速度计受了线性加速度干扰，二是真实姿态发生了剧烈变化。前者不该跟，后者必须跟。区分两者的手段是看r_measure设得合不合理，以及是否需要额外的野值剔除——如果|y|突然超过 30°，可以先不更新，等下一帧，这个技巧在振动大的场景里能明显改善。

kalman1d_init里P[0][0] = 1.0f而不是 0，是因为协方差为 0 意味着「我对初始状态完全确定」，增益会变成 0，滤波器从此再也不更新，看起来就像死掉了一样。这个错误我见过不止一次，现象是角度一直停在 0，怎么调参数都没反应。

双轴调用就是把roll和pitch各配一个kalman1d_t实例，用同一组参数，传入各自的陀螺轴和加速度观测角。两个轴独立滤波，互不干扰，这是六轴方案能成立的前提。

4.3 Q/R 调参：三个参数对应三种现象

参数只有三个，但每个都会导致完全不同的现象，按现象反推比盲试快得多。

调参的顺序我一般是这样：先把q_bias设得很小（比如 0.001），让零偏估计稳定下来；然后固定q_angle = 0.001，只调r_measure，从 0.001 开始往上加，直到静止时角度不再跟着振动抖；最后再回头微调q_angle，看动态跟随是否足够快。

判断标准不用猜，看三条曲线：陀螺纯积分角（一定在漂）、加速度反解角（一定在抖）、滤波输出角。理想结果是滤波输出既不漂也不抖，且在快速转动时延迟不超过 50ms。如果滤波输出跟着加速度角一起抖，说明r_measure太小；如果滤波器对真实转动反应迟钝、回正很慢，说明r_measure太大或者q_angle太小。

有个反直觉的经验：r_measure并不是越小越好，也不是越大越好，它应该和你的实际噪声水平匹配。测量方法很简单，把模块静置，采集 500 组加速度反解角，算标准差 σ，r_measure取 σ² 量级就是合理起点。这比拍脑袋设 0.03 靠谱得多，也是我觉得最值得养成的一个习惯——参数从数据里来，不从记忆里来。

5. 避坑与排查：五个让人熬夜的现场问题

5.1 静置时角度缓慢单向漂移

现象：模块放在桌上不动，串口打出来的角度十分钟内从 0 漂到 5° 以上，而且方向始终一致。

原因：陀螺零偏没标定，或者标定时模块没真正静止。1°/s 的残余零偏，十分钟就是 600°，即使滤波器在慢慢修正，q_bias太小的话修正速度也跟不上。

解决：上电强制静止标定 1000 组，标定完成后打印残余零偏值，正常应该在 ±0.5°/s 以内。如果标定后仍然偏，检查标定期间是否有风扇、空调气流或者桌面振动，另外确认0x6B写的是0x01而不是0x00，内部 RC 时钟的温漂会直接体现在这里。

5.2 电机一转，角度就剧烈抖动

现象：静止时角度很稳，一旦电机启动，roll 和 pitch 开始高频抖动，幅度能到 ±5°。

原因：电机振动通过机架传导到模块，加速度计把振动当成重力分量，反解出的观测角剧烈跳变；同时振动频率可能接近或超过 DLPF 截止频率，滤波没压住。

解决：三层手段一起上。硬件上给模块加软垫做机械解耦；配置上把 DLPF 从0x03调到0x05或0x06（截止频率更低）；算法上把r_measure调大，并在更新前加一道野值判断，|acc_angle - angle| > 25°时跳过本次更新。硬件解耦的效果最明显，优先做。

5.3 快速转动后姿态出现台阶式跳变

现象：快速转一圈再停住，角度不是平滑变化，而是在某个瞬间跳了几度，然后稳定在新值上。

原因：一是 I2C 读取分成了两次，加速度和陀螺不在同一时刻；二是dt用了固定值，实际间隔在快速处理时变长，积分量算少了；三是滤波器发散后重新收敛。

解决：改成一次突发读 14 字节；用微秒计时器取真实dt并做区间保护；在串口日志里把每帧的dt打出来，看是否有异常大的值。如果日志里出现 50ms 以上的间断，往上查是不是主循环里有阻塞操作，比如串口printf用了阻塞发送。

5.4 上电后角度一直是 0，参数怎么调都没反应

现象：代码跑了，数据也在更新，但滤波输出恒等于 0，或者停在初始值附近不动。

原因：协方差矩阵初始化为全 0，卡尔曼增益K = P/(P+R)算出来是 0，状态永远不更新。另一种可能是acc_angle传进来就是 0，比如atan2的参数顺序写反，或者原始数据没做符号扩展。

解决：把P[0][0]初始化为 1.0 左右；单独打印acc_angle确认它在静止时接近 0、倾斜时明显变化。原始数据这块特别容易出错：(buf[0] << 8) | buf[1]如果不强转成int16_t，负温度、负角速度会被解析成六万多的正数，现象是「往一个方向转正常，往另一个方向转数值突然变大」。

5.5 换一块模块，同样的代码效果差很多

现象：代码和参数一个字没改，换了一块同型号模块，漂移和噪声水平明显不同。

原因：MPU6050 是 MEMS 器件，个体差异客观存在，零偏和噪声水平出厂就有离散性；另外模块板的 LDO、去耦电容、走线布局不同，电源纹波会直接影响输出噪声。

解决：接受这个事实，把标定做成每次上电自动执行，而不是把某块模块的零偏硬编码进代码。如果批量产品，考虑在产线做一次标定并把偏置写入 flash，运行时读出来用。参数也要留出可配置接口，别把r_measure写死在函数体里——这个教训很直接，我曾经因为一个写死的常量，在换供应商之后整整排查了两天。

6. 二次开发：从单轴到双轴输出与上位机联调

单轴跑通之后，往下走通常有两个方向：一是把姿态输出做完整（双轴 + 四元数 + 串口协议），二是换更强的融合算法（Mahony、Madgwick，或者干脆上九轴）。这两个方向不冲突，但顺序建议是先做完前者。

双轴封装很轻，就是把两个滤波器实例和标定结果打包：

typedef struct { kalman1d_t k_roll; kalman1d_t k_pitch; float roll, pitch; /* 滤波后输出，单位度 */ float yaw_rate; /* 只有角速度，没有绝对航向 */ } attitude_t; void attitude_init(attitude_t *att) { /* 两个轴用同一组参数，先保证一致性，再按需分别微调 */ kalman1d_init(&att->k_roll, 0.001f, 0.003f, 0.03f); kalman1d_init(&att->k_pitch, 0.001f, 0.003f, 0.03f); }

yaw_rate这个字段是有意保留的：六轴拿不到绝对 yaw，但角速度本身是有用的，云台增稳、转向检测这类应用只关心角速度积分出来的相对转角，不需要绝对参考。把它明确命名成yaw_rate而不是yaw，是为了在接口层面提醒调用者「这里没有绝对航向」，避免上层误用。

如果确实需要绝对航向，路线有两条，选哪条取决于精度要求和成本：

零速修正这条路线经常被忽略，但在很多实际产品里性价比最高：检测到设备静止（三轴角速度都接近 0 且加速度模长接近 1g）时，直接把角度锁死并重置协方差，动态时正常积分。这样即使没有磁力计，长时间的静态漂移也能被彻底消掉。

上位机联调这块，我的习惯是定义一条最简的文本协议，一行一帧，字段用逗号分隔：roll,pitch,yaw_rate,dt,acc_roll。多打两个中间量（acc_roll和dt）看着啰嗦，但排查问题时这两个字段能省掉大量猜测。波特率 115200 在 100Hz 下每帧约 40 字节，带宽够用。PC 端用脚本读串口、画实时曲线，比盯着数字滚动强太多。

验证方法也顺便说一下：把模块固定在一个已知角度的斜面上（比如 30°），看滤波输出收敛后是不是 30°；然后快速翻转 90° 再静止，看超调和稳定时间。这两个测试能同时暴露标定误差和参数不匹配的问题，比随便晃两下靠谱。

最后说一句实在的教训。我早期做姿态项目，习惯先把参数调到自己觉得「看着挺顺」，然后就收工。后来在一次低温环境测试里，整个姿态系统漂到没法用，回头查才发现标定是在室温做的，q_bias又设得太小，滤波器根本跟不上温漂带来的零偏变化。从那以后我每次做姿态解算，都会强制走一遍「静置漂移测试、已知角度验证、温变复测」这三步，哪怕时间再紧也不跳。参数这东西，调出来的顺滑感是会骗人的，只有数据不会。希望帮到你。

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