STM32双IIC读取MPU6050与9250磁力计卡尔曼滤波姿态解算

简介：这是一套面向嵌入式开发者的STM32F1姿态解算工程源码与配套项目文件，整合了MPU6050（IIC1）与MPU9250（IIC2）双传感器数据读取、卡尔曼滤波融合及姿态角输出，适合需要从零实现俯仰、横滚、偏航角度和指南针角度的学习与移植场景。资源压缩包共179个文件，整体仅1.91MB，除C语言与C++源码、头文件外，还包含编译中间文件（o、axf、hex）和Keil工程配置文件（uvprojx、uvoptx）等，便于直接打开工程查看代码结构与构建逻辑。目前已有1067人学习，实用性经过验证。工程核心围绕IIC外设配置、寄存器读写、原始数据预处理及卡尔曼滤波展开，通过IIC1读取MPU6050的加速度与角速度，IIC2读取MPU9250的磁场数据，再融合输出稳定姿态角，同时提供指南针方向解算。通过阅读源码与配置，可以梳理从传感器初始化到姿态输出的数据链路，有助于理解传感器误差补偿与多源数据融合的完整流程。

1. 双 IIC 读 6050 和 9250 做姿态解算：这个标题到底在解决什么

先还原一个常见场景：四轴、云台或两轮平衡车上电后，pitch 和 roll 靠加速度计还能稳住，但 yaw 会随着时间慢慢飘，因为陀螺仪积分天生有零偏累积，转几圈后航向早就不是原来的方向了。这个标题给的思路很直接：STM32F1 用两路独立 IIC，一路从 MPU6050（IIC1）读加速度和角速度，另一路从 9250 磁力计（IIC2）读地磁数据，再用卡尔曼滤波把它们融合成稳定的 pitch、roll、yaw，并同时输出一个独立的指南针角度。适合正在做姿态解算、已经被“单靠陀螺积分会飘”这件事折腾过的开发者。读完你不仅能搭出一套可复现的读取链路，还能知道滤波参数到底怎么调、总线卡死和角度跳变这些坑从哪来。

2. 双 IIC 硬件连接与初始化：为什么 IIC1、IIC2 要分开

很多第一次接触这个方案的人会问：6050 和 9250 又不冲突，挂同一条总线上不行吗？答案是能挂但没必要。9250 的磁力计部分在初始化阶段对时序比较敏感，上电瞬间如果两个器件同时在总线上做内部配置，很容易把 SDA 拉低一段时间，导致 6050 的那次 I2C 访问直接失败。把两条总线分开，每个器件独享一套 SCL/SDA，调试时也能先单独验证 6050，再单独验证 9250，谁出问题一眼就能定位。

2.1 引脚分配、供电与上拉电阻

以 STM32F103 系列为例，I2C1 和 I2C2 的引脚是固定的：

供电上，MPU6050 模块如果板载稳压芯片，很多可以直接接 5V，没有稳压就必须接 3.3V；9250 磁力计我一般统一给 3.3V。两条总线的 SCL、SDA 都要接 4.7kΩ 上拉电阻到 3.3V，如果模块 PCB 上已经自带，就不用重复加。注意上拉电阻不是随便选，总线速度 100kHz 时可以用 10k，400kHz 时建议 4.7k 甚至 2.2k，太小会拉不动、太大则上升沿变缓。

2.2 用标准外设库初始化两路 I2C

STM32F1 的标准外设库里，I2C 初始化的套路比较统一，两路总线共用一套结构体配置，但要注意 I2C1 和 I2C2 的时钟都挂在 APB1 上，而 GPIOB 的时钟在 APB2 上，两个都要打开：

#include "stm32f10x_gpio.h" #include "stm32f10x_i2c.h" void MX_I2C_Init(void) { GPIO_InitTypeDef GPIO_InitStructure; I2C_InitTypeDef I2C_InitStructure; RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1 | RCC_APB1Periph_I2C2, ENABLE); GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7; // I2C1: PB6/PB7 GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD; // 复用开漏 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_Init(GPIOB, &GPIO_InitStructure); GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10 | GPIO_Pin_11; // I2C2: PB10/PB11 GPIO_Init(GPIOB, &GPIO_InitStructure); I2C_InitStructure.I2C_Mode = I2C_Mode_I2C; I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_16_9; I2C_InitStructure.I2C_OwnAddress1 = 0x30; I2C_InitStructure.I2C_Ack = I2C_Ack_Enable; I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; I2C_InitStructure.I2C_ClockSpeed = 400000; I2C_Init(I2C1, &I2C_InitStructure); I2C_Init(I2C2, &I2C_InitStructure); I2C_Cmd(I2C1, ENABLE); I2C_Cmd(I2C2, ENABLE); }

GPIO 必须配成复用开漏，因为 I2C 总线本身靠外部上拉电阻实现高电平，开漏输出才能让多个设备安全共享电平。I2C_ClockSpeed 写 400000 代表快速模式，如果两块模块的 PCB 布局比较长、线缆干扰大，降到 100000 更稳。OwnAddress1 只要不跟总线上的 0x68、0x0C 冲突就行，这里写 0x30 是个占位。DutyCycle 只有快速模式才需要配，100kHz 标准模式下这个参数无效。

2.3 软件 I2C 保底：60 行代码换来的稳定

使用 F1 系列原生的硬件 I2C 外设时，偶尔会遇到“读着读着总线就卡死”的玄学问题，这在后的避坑章节会细讲。很多实际项目的做法是：保留上面的硬件 I2C 初始化代码，但把寄存器读写的底层函数统一封装，方便在硬件 I2C 和软件 I2C 之间一键切换：

#define I2C_USE_SOFT 1 #if I2C_USE_SOFT uint8_t I2C1_ReadReg(uint8_t dev_addr, uint8_t reg) { // 用 GPIO 翻转模拟 I2C 时序，代码约 60 行 return Soft_I2C_ReadByte(IIC1_PORT, dev_addr, reg); } #else uint8_t I2C1_ReadReg(uint8_t dev_addr, uint8_t reg) { return HAL_I2C_Mem_Read(I2C1, dev_addr, reg, ...); } #endif

软件 I2C 的底层就是延时翻转 SCL、采样 SDA，实现一个起始信号、停止信号、字节收发，大概 60 行代码。我一般直接把两个传感器都用软件 I2C 跑 100kHz，牺牲一点速度换来信手拈来的稳定。硬件 I2C 能省中断资源，但前提是你愿意在每次通信失败后做外设复位处理。新手阶段，我更推荐先把软件 I2C 跑通，等姿态解算逻辑没问题了，再回过头优化成硬件 I2C。注意 9250 磁力计的数据更新率通常只有 100Hz 左右，软件 I2C 的速率完全够用。

3. 原始数据读取：6050 的六轴与 9250 的地磁

这一章是整个方案的数据源头。6050 提供三轴加速度和三轴角速度，9250 提供三轴地磁。数据读得准不准，直接决定卡尔曼滤波能不能收敛。很多人在滤波上花了好几天，最后发现是原始数据符号反了或者寄存器地址抄错，这种本末倒置的教训我见过太多次。

3.1 6050 初始化序列与量程选择

6050 上电后默认是睡眠状态，必须先唤醒，再配置采样率、数字低通滤波器和量程。以下是我常用的初始化序列：

void MPU6050_Init(void) { I2C1_WriteReg(MPU6050_ADDR, 0x6B, 0x00); // PWR_MGMT_1: 退出睡眠 I2C1_WriteReg(MPU6050_ADDR, 0x19, 0x07); // SMPLRT_DIV: 1kHz/(7+1)=125Hz I2C1_WriteReg(MPU6050_ADDR, 0x1A, 0x03); // CONFIG: DLPF 带宽约 44Hz I2C1_WriteReg(MPU6050_ADDR, 0x1B, 0x18); // GYRO_CONFIG: ±2000°/s I2C1_WriteReg(MPU6050_ADDR, 0x1C, 0x00); // ACCEL_CONFIG: ±2g }

0x19 寄存器写 7，表示陀螺仪内部 1kHz 采样率除以 8，得到 125Hz 的数据输出率，这个频率跟后面卡尔曼的 10ms 定时更新是匹配的。0x1A 的 DLPF 是数字低通滤波器，0x03 对应陀螺仪带宽约 44Hz、加速度计带宽约 42Hz，能滤掉一部分高频振动，又不会让响应太迟钝。量程的选择直接影响灵敏度：

姿态解算我习惯用 ±2000°/s 和 ±2g，前者保证快速翻转时角速度不削顶，后者让静止时加速度分辨率最高。如果做的是云台这种缓慢运动场景，陀螺量程降到 ±500°/s 更好，分辨率更高。

3.2 读取 6050 原始数据并换算成物理量

6050 的加速度和角速度数据从寄存器 0x3B 开始连续排列，一次突发读取 14 个字节即可拿到全部六轴数据和温度。转换成物理量的关键是把两个 uint8_t 拼成有符号 int16_t，这一步最容易翻车：

typedef struct { float ax, ay, az; // 单位: g float gx, gy, gz; // 单位: °/s } MPU6050_Data; void MPU6050_Read(MPU6050_Data *d) { uint8_t buf[14]; I2C1_ReadRegs(MPU6050_ADDR, 0x3B, buf, 14); int16_t raw_ax = (int16_t)((buf[0] << 8) | buf[1]); int16_t raw_ay = (int16_t)((buf[2] << 8) | buf[3]); int16_t raw_az = (int16_t)((buf[4] << 8) | buf[5]); int16_t raw_gx = (int16_t)((buf[8] << 8) | buf[9]); int16_t raw_gy = (int16_t)((buf[10] << 8) | buf[11]); int16_t raw_gz = (int16_t)((buf[12] << 8) | buf[13]); d->ax = raw_ax / 16384.0f; d->ay = raw_ay / 16384.0f; d->az = raw_az / 16384.0f; d->gx = raw_gx / 16.4f; d->gy = raw_gy / 16.4f; d->gz = raw_gz / 16.4f; }

强制转换成 int16_t 这一步不能省，因为 buf[0] 是 uint8_t，直接左移 8 位后整个表达式会按 int 处理，当温度或加速度为负时，高位补符号位的逻辑会出错，组合出来的数值和你预期的完全不一样。换算系数必须和前面配置的量程对应：±2g 除以 16384，±2000°/s 除以 16.4。读出来的数据单位统一到了物理量，后面卡尔曼滤波就不用再关心量程问题。

3.3 读取 9250 磁力计并算出指南针角度

标题里说的 9250 磁力计，我按常见九轴模组的用法处理——只用它内部的磁力计部分，I2C 地址是 0x0C。不同批次模块的寄存器偏移偶尔有差异，建议先读一下 0x00 的器件 ID 确认地址正确，再配置连续测量模式：

#define MAG9250_ADDR 0x0C void MAG9250_Init(void) { uint8_t id = I2C2_ReadReg(MAG9250_ADDR, 0x00); // id 正常应读到 0x48 附近，读不到先查上拉和供电 I2C2_WriteReg(MAG9250_ADDR, 0x0A, 0x16); // 连续测量模式，16位输出 I2C2_WriteReg(MAG9250_ADDR, 0x0B, 0x08); // 数据更新率约 100Hz } void MAG9250_Read(int16_t *mx, int16_t *my, int16_t *mz) { uint8_t buf[6]; I2C2_ReadRegs(MAG9250_ADDR, 0x03, buf, 6); *mx = (int16_t)((buf[0] << 8) | buf[1]); *my = (int16_t)((buf[2] << 8) | buf[3]); *mz = (int16_t)((buf[4] << 8) | buf[5]); }

之所以把 9250 单独放 IIC2，除了避免上电时序互相干扰，还因为磁力计的读数更新率低，如果和 6050 挤在同一条总线上，突发读取时容易互相拖慢。读出三轴磁场原始值后，还不能直接用 atan2 算角度，因为 PCB 上的焊盘、附近的金属结构会给磁力计叠加一个固定偏置，这叫硬铁误差。先让板子水平旋转一整圈，采集 200 个点，记录每个轴的最大值和最小值，取中点作为偏移量：

typedef struct { int16_t mx_off; int16_t my_off; } MagCalib; void MagCalib_Run(MagCalib *c) { int16_t minx = 32767, maxx = -32768; int16_t miny = 32767, maxy = -32768; for (int i = 0; i < 200; i++) { int16_t mx, my, mz; MAG9250_Read(&mx, &my, &mz); if (mx < minx) minx = mx; if (mx > maxx) maxx = mx; if (my < miny) miny = my; if (my > maxy) maxy = my; delay_ms(10); } c->mx_off = (minx + maxx) / 2; c->my_off = (miny + maxy) / 2; } float Mag_Heading(int16_t mx, int16_t my, const MagCalib *c) { float fx = mx - c->mx_off; float fy = my - c->my_off; float deg = atan2f(fy, fx) * 57.29578f; if (deg < 0) deg += 360.0f; return deg; // 单位: 度，0° 表示磁北 }

atan2f 返回的是弧度，乘 57.29578 转成角度后范围在 -180° 到 180°，加 360 再取模就变成了 0° 到 360° 的指南针角度。这个角度默认指向磁北，不是真北，两者之间的差叫磁偏角。不同地区的磁偏角差异很大，一般从当地查表获取，大约在 -10° 到 +10° 之间，只需要在程序里存一个常量，真航向等于磁航向加磁偏角。注意如果板子不水平，指南针角度会有倾斜误差，简单场景下可以先用 pitch、roll 做倾斜补偿，或者干脆保持水平测试。

4. 卡尔曼滤波融合：从原始数据到 pitch、roll、yaw

原始数据到手后，最核心的问题来了：怎么把加速度计、陀螺仪、磁力计融合成稳定的欧拉角。加速度计长期稳定但动态响应差，陀螺仪短期精准但会漂，磁力计能纠正航向但怕干扰。卡尔曼滤波在每一步都根据传感器噪声特性动态分配权重，这就是这个标题选择它的原因。

4.1 为什么不是互补滤波：卡尔曼的好处与代价

互补滤波的经典写法是一行公式：angle = 0.98 * (angle + gyro * dt) + 0.02 * acc_angle。它的思路是让高频可信的陀螺仪主导短期变化，让低频可信的加速度计校正长期漂移，简单直接，确实能跑。但互补滤波有一个明显短板：陀螺仪零偏只能靠加速度计慢慢拉，在线性加速或剧烈振动时，加速度计不可信，零偏就没法修正。卡尔曼滤波把“陀螺仪零偏”也作为一个状态量去估计，每来一帧观测，它不只修正角度，还修正零偏，相当于一边用角速度积分、一边实时校准积分起点。代价是代码量多一些，还要理解几个参数的含义，但对于需要稳定 yaw 的场景，多出来的几十行代码非常值。

4.2 两状态卡尔曼实现与角度融合代码

对单轴角度，一个两状态卡尔曼就足够：状态向量是 [角度, 陀螺零偏]。下面这个实现是可独立运行的，pitch、roll、yaw 各维护一个实例：

typedef struct { float angle; // 融合后的角度，单位弧度 float bias; // 陀螺零偏，单位弧度/秒 float P[2][2]; // 协方差矩阵 } Kalman_t; void Kalman_Init(Kalman_t *k, float angle_init) { k->angle = angle_init; k->bias = 0.0f; k->P[0][0] = 0.0f; k->P[0][1] = 0.0f; k->P[1][0] = 0.0f; k->P[1][1] = 0.0f; } void Kalman_Update(Kalman_t *k, float gyro_rate, float innovation, float dt, float Q_angle, float Q_bias, float R_meas) { // 预测：陀螺积分 + 协方差递推 k->angle += (gyro_rate - k->bias) * dt; k->P[0][0] += dt * (dt * Q_angle + k->P[1][1]); k->P[0][1] -= dt * k->P[1][1]; k->P[1][0] -= dt * k->P[1][1]; k->P[1][1] += Q_bias * dt; // 观测更新 float S = k->P[0][0] + R_meas; float K0 = k->P[0][0] / S; float K1 = k->P[1][0] / S; k->angle += K0 * innovation; k->bias += K1 * innovation; float P00 = k->P[0][0]; float 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; }

调用时，pitch 和 roll 的观测值来自加速度计解算出的角度，yaw 的观测值来自磁力计指南针角度：

// 加速度计解算 roll、pitch，单位弧度 float roll_acc = atan2f(d.ay, d.az); float pitch_acc = atan2f(-d.ax, sqrtf(d.ay * d.ay + d.az * d.az)); Kalman_Update(&kp, d.gx * DEG2RAD, pitch_acc - kp.angle, dt, 0.001f, 0.003f, 0.03f); Kalman_Update(&kr, d.gy * DEG2RAD, roll_acc - kr.angle, dt, 0.001f, 0.003f, 0.03f); // yaw 的观测误差必须先做角度归一 float innov = atan2f(sinf(mag_rad - ky.angle), cosf(mag_rad - ky.angle)); Kalman_Update(&ky, d.gz * DEG2RAD, innov, dt, 0.001f, 0.003f, 0.05f);

滤波器更新完，kp.angle、kr.angle、ky.angle 就是融合后的 pitch、roll、yaw，再乘回去转成角度输出。这里最需要注意的是 yaw 的观测误差，磁力计指南针角度和融合角度可能在 ±180° 附近跳变，直接相减会得到接近 360° 的巨大误差，所以必须用 atan2f(sinf, cosf) 把误差归一化到 -π 到 π 之间。

4.3 参数怎么调：Q、R、dt 的量纲规律

卡尔曼参数不是玄学，但还是有量纲逻辑的。Q_angle 是角度过程噪声方差，单位 rad²，它影响滤波器对陀螺仪积分的信任程度；R_meas 是观测噪声方差，单位也是 rad²，它表示加速度计或磁力计测得的角度有多可信；Q_bias 控制零偏估计的跟踪速度。我常用的起点是 Q_angle = 0.001，Q_bias = 0.003，R_meas = 0.03。如果发现角度太迟滞，快速转动跟不上，就把 Q_angle 调大到 0.01，或者把 R_meas 调小到 0.01；如果静止时角度抖动太厉害，就把 R_meas 调大到 0.1。dt 必须和实际定时器中断周期严格一致，比如 10ms 就是 0.01f，不能用 main 循环里的延时凑数，因为循环时间会随代码路径变化，dt 抖动会被卡尔曼当成强烈的动态输入，表现为静止时角度也在小幅跳变。

5. 姿态解算的常见问题排查：5 条血泪经验

这一章全部是实打实踩过的坑，每条按现象、原因、解决的顺序写。如果你照着前面的代码做还是跑不出稳定角度，大概率问题出在这五件事里。

5.1 硬件 I2C 总线挂死，读几秒就卡

现象：上电第一次读 6050 正常，循环读几十次后程序卡死在 I2C 等待事件的地方，复位后又能跑一会儿。原因：STM32F1 的硬件 I2C 外设在从机时钟拉伸或总线上有毛刺时，BUSY 标志会一直置位，主设备发送起始信号时发现总线忙，就不往下走了。解决：最快的方案是切到软件 I2C；如果一定要用硬件 I2C，每次通信失败后做一次软复位：

if (I2C_GetFlagStatus(I2C1, I2C_FLAG_BUSY) == SET) { I2C_Cmd(I2C1, DISABLE); I2C_SoftwareResetCmd(I2C1, ENABLE); I2C_SoftwareResetCmd(I2C1, DISABLE); I2C_Cmd(I2C1, ENABLE); }

这个操作会清掉总线状态机的所有残留状态，代价是当前这次通信作废，丢一帧数据。对姿态解算来说，125Hz 更新率下丢一帧完全感知不到。

5.2 组合寄存器数据符号错乱，角度乱跳

现象：加速度计读出的 az 在水平放置时是负数，或者翻转板子时角度方向反着走。原因：uint8_t 组合成 int16_t 时没有做类型转换，高字节最高位本来是符号位，结果被当成无符号数拼进去，数值就直接偏了 512 以上的偏移。还有一个常见原因是 atan2f 的参数顺序写反了，导致角度象限判断错误。解决：先打印原始寄存器值的十六进制，水平放置时 az 的高字节应该在 0x3F 到 0x40 附近，对应 +16384 左右。如果方向反了，对调 atan2f 的两个参数试试，或者把对应轴的数据整体取负。

5.3 yaw 在 ±180° 附近来回震荡

现象：板子朝向磁北附近时，滤波输出的 yaw 在 179° 和 -179° 之间反复跳，其他方向都正常。原因：磁力计指南针角度输出范围是 0° 到 360°，而卡尔曼内部用弧度表示角度，范围是 -π 到 π。当真实角度接近 180° 时，滤波值和观测值一个在 179°、一个在 -179°，直接相减得到 358°，滤波器以为发生了剧烈变化，疯狂往错误方向修正。解决：进入滤波器之前把观测误差归一化到 -180° 到 180°：

float err = mag_rad - ky.angle; err = atan2f(sinf(err), cosf(err));

这个 wrap 操作是 yaw 融合里绝对不能省的一步，省了就是整晚在 ±180° 附近反复横跳的结局。

5.4 卡尔曼滤波发散成几十万

现象：静止放置，pitch 角度在两三秒内从 10° 涨到 80°，甚至变成几千度。原因：最常见的是量纲混用。陀螺仪数据是度每秒，加速度计解算的角度是度，但卡尔曼内部按弧度计算，一混用，每次积分都放大 57 倍。另一个原因是角速度符号反了，陀螺仪积分方向跟实际旋转方向相反，观测值一进来就被当成异常误差猛拉，滤波器直接发散。解决：进滤波器前把所有角度统一成弧度，陀螺仪数据乘 DEG2RAD，加速度计解算结果本身是弧度。然后打印一下陀螺仪数据和加速度计解算角度，板子绕 x 轴转，两者变化方向必须一致。我习惯把中间变量全部打出来，而不是直接看最终角度，这是排查滤波问题最有效的方法。

5.5 磁力计受干扰，指南针角度跟着抖

现象：电机一转，yaw 马上剧烈抖动；电机停转后慢慢恢复。原因：磁力计对磁场变化极度敏感，电机绕组和大电流走线附近的磁场会直接叠加在地磁信号上，另外电源纹波也会通过供电引脚窜进来。解决：硬件上让 9250 尽量远离电机和大电流线，磁力计电源入口加一个 10Ω 电阻加 10μF 电容的 π 型滤波；软件上对三轴磁力计原始数据做滑动平均，取最近 5 个点的均值再算航向。如果动态场景要求跟手，可以把 R_meas 在转动时调大，让滤波器临时更相信陀螺仪，静止时再恢复默认值。

6. 验证与进阶：串口波形核对与上电校准

6.1 用串口协议加波形工具核对三个角度

整套系统能不能用，不能靠眼睛看板子猜。我一般把调试帧固定在 50Hz 输出，格式就一行文本：

printf("P:%.1f R:%.1f Y:%.1f M:%.1f\r\n", pitch_deg, roll_deg, yaw_deg, mag_heading);

然后接到串口波形工具里，把四条曲线都显示出来。静止时四条线都应该是平直的，pitch 和 roll 在 0° 附近，yaw 和 M（指南针角度）重合。绕 x 轴翻转板子，pitch 应该跟随一个斜坡，roll 保持不动；绕 z 轴旋转，yaw 和 M 同步变化。如果哪条曲线不跟随对应轴的动作，第一步不是查卡尔曼，而是回到第三章确认那个轴的原始符号方向。曲线跟手且无漂移，这套链路才算真正跑通。

6.2 上电自动零偏校准与动态 R 技巧

卡尔曼虽然能估计陀螺零偏，但收敛需要时间，而且收敛期间 yaw 可能有几度的初始偏差。我的习惯是上电后强制板子静止 1 秒，把这段时间的陀螺仪 z 轴均值作为卡尔曼 bias 的初始值，而不是从 0 开始：

for (int i = 0; i < 100; i++) { MPU6050_Read(&data); gz_sum += data.gz; delay_ms(10); } Kalman_Init(&ky, mag_heading_rad); ky.bias = gz_sum / 100.0f * DEG2RAD;

另一个进阶技巧是动态调整 yaw 的 R_meas。静止时磁力计很可靠，R_meas 用 0.05；当 |gz| 超过 30°/s 时，说明正在快速转弯，磁力计的更新率和倾斜误差会让观测值滞后，这时把 R_meas 临时调到 0.2，让 yaw 优先信任陀螺仪，转完再恢复。

这套双 IIC 加卡尔曼的方案，是我在某个模拟项目 X 上从零调通的。回顾整个过程，最耗时间的不是滤波，而是 I2C 卡死和 yaw 边界跳变这两个小问题。现在我把所有角度统一成弧度、误差一律先归一到 ±π，总线改用软件 I2C 后，这套逻辑基本不需要再动。希望这篇文章能让你少走这些弯路，把时间花在真正有价值的姿态控制上，而不是跟传感器数据搏斗到凌晨。

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