1. 从传感器噪声说起：为什么MCU开发者迟早会碰卡尔曼
1.1 直接读传感器原始数据是什么体验
做嵌入职控的同行应该都有过这种体验：手里拿着一块带六轴IMU的开发板，想测个角度做平衡小车或者云台，一读原始数据发现加速度计的噪声大到让人怀疑传感器是不是坏了。
实测过一块常见的MPU6050模块，静止放在桌面上，以100Hz采样读取加速度计的X轴，数值在±0.05g左右来回跳。换算成角度，静止状态下直接由加速度计算出来的倾角能有超过5度的波动。这个噪声水平对于平衡控制来说是不能忍的，直接拿来用，电机和舵机会被抖得嗡嗡响。
陀螺仪这边是另一番景象。它响应快、高频噪声相对小，但它测的是角速度，想得到角度必须做积分。只要积分，误差就会随时间累积，积分两三秒可能还能看，超过十秒就明显偏出去了。温度漂移、零点偏移、量程非线性都会变成积分误差的一部分。
这就引出一个典型困境：加速度计在静态下可靠但高频噪声大，陀螺仪动态跟随好但长期漂移。任何只靠单一传感器做姿态角估计的方案都会在某个维度上翻车。
1.2 低通滤波和互补滤波解决不了的问题
有人会先说，给加速度计角度加个低通滤波不就行了。确实能压住高频抖动，代价是相位滞后。你做云台，云台一摆动，滤波后的角度慢半拍，控制环就跟着震荡，越补越大。
互补滤波是更常见的轻量方案：把陀螺仪积分出的角度做高通，把加速度计角度做低通，然后按比例叠加。形式上就是一句经典的：
angle = 0.98f * (angle + gyro * dt) + 0.02f * acc_angle;这个方案在一阶近似上没问题，计算量几乎为零。但问题是那0.98和0.02是固定的折中，传感器噪声变大时它不会自动调整权重，运动加速度干扰变大时它也不知道多信任陀螺仪一点。整个滤波器在"平滑"和"跟随"之间只能做一个静态取舍，换一个场景就得重新调这两个数。
卡尔曼滤波器在这里的本质优势，是它能根据当前的不确定度动态调整"信模型多一点还是信测量多一点"，而且这个权重是逐时刻计算出来的。对于姿态角这类低维问题，它并不像有些人想象中那么贵。2x2矩阵运算放在STM32上完全跑得动。
1.3 STM32上卡尔曼的适用边界
如果你要处理的是两三个状态量的线性系统，比如姿态角估计、电池SOC估算、温度传感器平滑、GPS轨迹去噪，卡尔曼滤波在STM32上是很合适的。状态维度低，每周期就是几十次乘法和几次除法，哪怕是STM32F103这种Cortex-M3内核，也能轻松支撑200~500Hz的更新率。
但也要把边界说清楚。SLAM里的高维状态估计、强非线性场景，比如大角度下的姿态解算，标准卡尔曼滤波器是扛不住的，那需要扩展卡尔曼或粒子滤波，而且内存和浮点开销会明显上升。做工程选择的时候，不要觉得卡尔曼四个字就能包打一切，它最适合的恰好就是这种"小状态、单测量、模型清晰"的场景。
2. 五个公式的工程解读：不是背公式，是理解状态怎么走
2.1 状态向量选什么：以角度和陀螺零漂为例
按标题的落点，这里直接用姿态角估计来拆解。卡尔曼滤波需要先定义一个状态向量x，它描述了系统在某个时刻的完整内部状态。不要只放角度一个量，那样陀螺仪零漂问题就没人管了。
最常用的做法是定义两个状态：
x = [ angle ] [ bias ]angle是我们要的融合角度，bias是陀螺仪的零漂估计。为什么把零漂也放进状态？因为陀螺零漂不是恒定值，它随温度变化、随供电电压变化。与其每次上电标定，不如让滤波器把它当成一个"慢变量"实时估计出来。状态里多一个变量，付出的运算代价可以忽略，但实际效果提升非常明显。
系统的运动方程写成：
angle_new = angle + dt * (gyro_rate - bias) + w bias_new = bias + w_bias第一行说：角度等于上一时刻的角度，加上陀螺仪角速度扣除零漂后的累积，再加上过程噪声w。第二行说：零漂本身缓慢变化，每次估计都允许它动一点点。
测量方程则是：
acc_angle = angle + v加速度计量出来的是一个被噪声污染的角度，它跟真实角度之间差一个测量噪声v。整个卡尔曼滤镜要做的，就是从这条又脏又能用的测量路线上，不断把状态修正到真实值附近。
2.2 预测阶段和更新阶段各做了什么
卡尔曼滤波的五个公式正好分成两组。前两个是预测阶段，后三个是更新阶段。
预测阶段做的事情，是用运动模型沿着时间往前推一步。一个形象的比喻是开车过隧道：在隧道里你看不到路标，只能根据车速和时间推算大概开了多远。这个推算结果肯定有误差，关键是还要把误差的"大小"也同步推算出来。
代码里的状态估计和协方差矩阵P的更新，就是在干这两件事。P矩阵的对角线元素代表每个状态量的不确定度方差，预测阶段P会变大，因为仅仅靠模型推算，时间越长越没把握。
更新阶段则像是出隧道后看到路牌，拿测量值校准。卡尔曼增益K计算的是"该拿多少测量值修正模型预测"，然后状态和P矩阵都被更新。更新后的P矩阵会变小，因为测量带来了新的信息，不确定性降低了。
一个周期结束时，滤波器已经为下一个周期的预测做好了准备。这就是五个公式的全部流程。
2.3 卡尔曼增益为什么是自适应的
很多人第一次看到K的计算公式会觉得抽象。其实卡尔曼增益就是一个动态权重，它告诉系统：当前时刻，模型预测和测量值谁更可信。
从公式结构看：
K = P_pred / (P_pred + R)如果P_pred很大，说明模型预测很不确定，此时K就接近1，系统会大比例采纳测量值。反过来如果R很大，说明测量噪声很剧烈，K就接近0，系统几乎只听模型输出。
这件事和互补滤波的固定系数有一个本质差异：互补滤波的0.98和0.02是写死的，而卡尔曼增益每一个采样周期都会重新计算。某个瞬间运动加速度干扰让加速度计测角不可信了，R虽然是常量，但P_pred在运动模型层面会相应变化，K会自己往模型那边偏。这种适应性，是它在工程上替代互补滤波的核心原因。
3. STM32平台实现前的三件准备工作
3.1 浮点还是定点的判断依据
STM32家族差异不小，F1、F3系列是Cortex-M3/M4没有硬件FPU，F4、F7以及部分G系列带硬件浮点单元。这直接决定你写浮点运算时要不要心疼内核。
如果是F4以上的芯片，直接用float没有任何压力。卡尔曼的2维状态每周期就几十次浮点乘加，硬浮点一条指令处理完，延迟可以忽略。如果是F1系列，软件浮点确实慢，但这里有个经验数据：2维卡尔曼一整套预测加更新，在72MHz主频下软件浮点大约几十微妙到一百多微妙。只要你的采样率在1kHz以内，依然不是瓶颈。
只有在极端场景下才需要做定点化，比如主频很低、中断预算很紧、或者需要对量化误差严格可控的场合。定点化一般用Q15或Q24格式，需要自己处理乘法后的缩放和溢出，调试成本高，收益在小状态场景下并不大。我的建议是：没有实测到性能瓶颈之前，不要在定点上浪费时间。
3.2 矩阵运算自己写还是用库
卡尔曼滤波的通用形式里涉及矩阵乘法和矩阵求逆。但具体到姿态角估计这个场景，状态是2维的，测量是1维的，增益计算里所谓的"矩阵求逆"实际上变成了一个标量除法。
也就是说，你根本用不上通用矩阵求逆算法。S = P[0][0] + R_measure这一个式子就完成了求逆的工作。这也是为什么很多IMU例程里的卡尔曼滤波代码只有几十行，而不是一个庞大的矩阵运算库。
如果将来你要把状态扩展到3维以上，比如增加一个水平速度估计，或者传感器融合不只一路，那时候再考虑自写一个轻量矩阵层，或者移植一份精简的C矩阵库也行。只有一个原则要守住：MCU上不要用动态内存分配，所有矩阵用静态数组定义好尺寸。
3.3 算力与内存的粗略预算
给一个可参考的账。2维状态、1维测量，每周期需要的所有浮点运算加起来在几十次乘加以内，加上3~4次除法。内存上，P矩阵是2x2共4个float，状态向量2个float，外加几个局部变量，全部加起来不超过100字节。
如果把这个账摊到具体芯片上：STM32F103在72MHz下软件浮点运行整套滤波，实测大约几十微秒到100微秒；STM32F405在168MHz下硬件浮点跑，几个微秒就能完成。相比传感器读取、滤波、通信、显示这些环节，卡尔曼滤波器根本不构成瓶颈。
所以做选型判断时，核心其实不是"STM32跑不跑得动卡尔曼"，而是"你的采样周期能不能稳定在设定值"。后面踩坑部分会专门讲这个问题。
4. 完整可抄作业：MPU6050姿态角卡尔曼滤波的C语言实现
4.1 数据结构与初始化
先把滤波器的数据结构定义好。一个结构体装状态、协方差矩阵和三个噪声参数，这是最容易维护的方式。
/* kalman.h */ #ifndef __KALMAN_H #define __KALMAN_H typedef struct { float Q_angle; /* 角度过程噪声方差 */ float Q_bias; /* 陀螺零漂过程噪声方差 */ float R_measure; /* 测量噪声方差 */ float angle; /* 估计出的角度状态 */ float bias; /* 估计出的陀螺零漂 */ float P[2][2]; /* 误差协方差矩阵 */ } Kalman_t; void Kalman_Init(Kalman_t *kalman); float Kalman_Update(Kalman_t *kalman, float measurement, float gyro_rate, float dt); #endif初始化时注意一个细节：P矩阵的初始值不要一律设0。如果启动时对初始角度没有把握，比如设备是上电后才被摆到某个姿态，P应该给一个较大的初值，比如0.5~1.0，让滤波器在前几个周期快速收敛到测量值。如果你能保证上电时设备处于已知水平姿态，P设0是没问题的，滤波器会从一开始就非常信任模型。
4.2 核心更新函数
下面是完整的核心逻辑，这段代码是整个项目最值得反复看的部分：
/* kalman.c */ #include "kalman.h" void Kalman_Init(Kalman_t *k) { /* 这三个参数是整定的起点，后面会细讲 */ k->Q_angle = 0.001f; k->Q_bias = 0.003f; k->R_measure = 0.03f; k->angle = 0.0f; 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; } float Kalman_Update(Kalman_t *k, float measurement, float gyro_rate, float dt) { /* ---- 预测阶段 ---- */ float rate = gyro_rate - k->bias; float angle_pred = k->angle + dt * rate; 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; float K0 = k->P[0][0] / S; float K1 = k->P[1][0] / S; /* ---- 用测量值更新 ---- */ float y = measurement - angle_pred; k->angle = angle_pred + K0 * y; k->bias = k->bias + K1 * y; /* ---- 更新协方差矩阵 ---- */ k->P[0][0] -= K0 * k->P[0][0]; k->P[0][1] -= K0 * k->P[0][1]; k->P[1][0] -= K1 * k->P[0][0]; k->P[1][1] -= K1 * k->P[0][1]; return k->angle; }注释里已经写清楚了四个阶段。预测阶段用陀螺仪角速度把角度推进一步，同时更新P矩阵；然后计算增益；然后用加速度计测量角度做一个修正；最后把协方差收窄。
这里最容易被忽略的是单位一致性。很多例程陀螺仪读取的是 deg/s，量角器量出来是 deg，然后算法还能跑，那是因为只要状态和测量单位一致，公式自洽。但如果你混用了 rad 和 deg，或者把 dt 当成毫秒代入公式，结果会发散的毫无悬念。我习惯全部统一为 rad、rad/s、秒，串口调试时再换算成度。
4.3 在主循环或中断中的调用时序
滤波器的调用时序比滤波代码本身更容易出事。最推荐的做法是在定时器中断里固定周期执行：
/* 假设定时器配置为2ms中断一次，500Hz */ volatile uint32_t tick_prev = 0; void TIM_Handler(void) { float gx = imu_read_gyro_x(); /* rad/s */ float acc_angle = imu_calc_acc_angle(); /* 由加速度计计算角度，rad */ float dt = (float)(tick_now - tick_prev) * 0.001f; tick_prev = tick_now; float angle = Kalman_Update(&kalman, acc_angle, gx, dt); /* 拿angle去做控制或者发送出去 */ }acc_angle的计算，最常见是取加速度计的X和Y轴，视旋转轴而定。如果是绕X轴测俯仰角，可以用：
float acc_pitch = atan2f(acc_y, acc_z);别用asin，因为atan2在任意象限下都稳定，而asin在大角度时会丢失符号信息，角度超过90度直接出错。这一点算是踩过的实在坑。
dt那个部分，很多人图省事直接写0.002f，但如果代码里某个环节耗时波动，实际采样周期就不是2ms，角度误差就会增大。取实际时间戳差的写法只增加两行代码，建议养成习惯。
5. Q和R矩阵的整定：这块真的靠手感
5.1 用测量噪声方差给R定初值
R_measure代表的是一次加速度计测角值本身携带的噪声方差。它不玄，可以直接测量。把开发板完全静止放平，以你计划用的采样率连续采集几百帧加速度计测角数据，算一下这组数据的方差，就是这个R的合理起点。
拿我之前测的MPU6050来说，静止时加速度计算出的俯仰角标准偏差大约0.1~0.2弧度，方差就是0.01~0.04。所以我初始R取0.03，跟实测吻合，后面不需要怎么动。
R取小了，滤波器会认为测量很准，结果就是加速度计的高频抖动被放行一部分，角度输出毛糙。R取大了，滤波器会认为测量不值得信，角度平滑了但动态响应变慢，快速摆动时会明显滞后。一个相对好用的经验：R取实测方差的1到3倍之间，然后根据观察到的抖动程度微调。
5.2 Q矩阵每个元素的物理含义
Q矩阵比R更容易让人懵，因为它不能直接测量。它描述的是：运动方程模型本身与真实物理过程之间的偏差。
Q_angle对应的是"角速度积分到底准不准"。你的陀螺仪测量角速度有噪声，dt近似、忽略某些加速度影响，这些误差累积起来，转换成角度的不确定性。Q_angle越大，滤波器越认为模型不可信，于是越依赖加速度计测量，输出会显得更"跳"。Q_angle越小，滤波结果越平滑，但可能对快速运动反应迟钝。
Q_bias对应"陀螺零漂自身的变化快慢"。如果陀螺零漂随温度剧烈变化，Q_bias要适当给大，让滤波器敢跟着修正bias；如果零漂很稳定，Q_bias给太大反而会让bias状态被噪声带着乱跳。
整定顺序一般这样走：先固定R，把Q_angle从0.0001开始逐渐加大，观察动态跟随是否有明显滞后；然后固定Q_angle调Q_bias，观察长时间静置时角度是否有缓慢漂移。每次只动一个参数，不要同时乱调。
5.3 整定过程中的典型异常现象
遇到异常先不用怀疑代码，很多时候是参数问题。几个常见现象和调整方向列个表，调试时可以对着查：
| 现象 | 可能原因 | 调整方向 |
|---|---|---|
| 静止时角度抖动大 | R偏小或Q偏大，太信任测量 | 增大R，或减小Q_angle |
| 快速转动时角度滞后 | R偏大或Q_Q角度偏小，响应太慢 | 减小R，或增大Q_angle |
| 长时间静置角度缓慢漂移 | Q_bias太小，零漂修正不及时 | 增大Q_bias |
| 输出角度出现明显突跳 | 加速度计测角异常或初始P太大 | 检查传感器滤波和初始化P |
| P矩阵变成NaN | dt为0或单位不一致 | 检查采样周期和单位 |
如果角度输出突然爆炸，第一步不是调参数，而是打印状态里的P矩阵，看是不是某个元素变成NaN或者急剧放大。八成是dt算错或者某个时刻传入的gyro_rate是异常值。
6. 实测踩坑记录：从发散到稳定的调试心得
6.1 快速转动时的滞后问题
第一次跑通算法的时候，拿在手上快速晃动，明显感觉到融合角度慢了半拍。当时以为是滤波代码写错了，查了整整一个下午，最后发现纯粹是R=0.03偏大，Q_angle=0.001偏小。系统太信任模型，导致测量修正力度不够。
把R降到0.01，Q_angle提到0.005，滞后立刻减轻。所以遇到动态响应问题，不要先折腾代码，先量化一下你实际场景里的噪声水平，数据的说话权永远比感觉强。
6.2 静置漂移回弹
还有一个特别容易踩的场景：设备静置不动，融合角度确实很稳，但过了几十秒后角度会慢悠悠漂出去一两度，然后突然跳回原位。这种"漂移后跳回"的现象，通常是陀螺仪零漂估计没有被充分激励导致的。
补偿一下思路。陀螺零漂估计需要系统在运动中采集足够信息，如果一直静止，加速度计测角没变化，bias状态长期得不到有效更新，而实际零漂在缓慢变化，积分误差就一点点积累起来。跳出这个坑的方法：给Q_bias适当加量，让零漂的随机游走在静止时也能被修正；同时确保加速度计数据进滤波前做过低通处理，避免传感器噪声污染bias的修正量。
6.3 采样周期抖动对滤波效果的影响
卡尔曼滤波公式里的dt是时变系统的关键参数，它必须跟实际采样间隔一致。之前有个项目为了提高主循环效率，把滤波器调用放在了while(1)循环里，用delay(2)大致控制节奏。结果串口打印出实际调度间隔在3ms到5ms之间剧烈波动，滤波器角度输出噪声明显变大。
那是我印象最深的一次排查。最终改成了定时器中断统一触发传感器读取和滤波更新，dt用时间戳差值而非固定常数。说句实在话，一套好的滤波算法，输送给它的时间戳不好，最后效果一定打折扣。数据质量、采样节律、滤波参数，三者缺一不可。
6.4 一点个人经验
最后把陪着我走过几个项目的小技巧分享出来。第一，调参之前先做数据回放，把原始加速度计角度、原始陀螺仪积分角度、卡尔曼输出角度一起用串口发到PC端画曲线，一眼就能看出问题出在哪个环节。第二，Q和R不必追求教科书式的精细建模，工程上标量就够用，真正影响体验的只有"平滑度"和"跟随速度"的平衡点。第三，在PC上先用双精度仿真一遍算法再移植到MCU，很多时候误差不是滤波公式产生的，而是浮点精度或者单位换算造成的。
把这些环节理顺之后，卡尔曼滤波器在STM32上就不再是纸面上的公式，而是一个稳定、轻量、可调的工具。希望这篇文章能让你少走一些弯路。
