简介一套基于STM32F103的卡尔曼滤波算法实现工程包面向嵌入式开发、控制算法学习及信号处理应用场景旨在解决在Cortex-M3微控制器上完成系统状态估计与测量噪声滤波的工程落地问题。压缩包共146个文件、3.52MB目录中包含38个.h头文件、37个.c源文件以及Keil工程配置文件.uvprojx/.uvoptx、链接脚本、编译生成文件.axf/.hex/.map等构成一套完整可编译运行的STM32固件工程有助于直接阅读源码、查看编译中间结果并快速移植。已有156人学习浏览适合具备一定STM32基础、希望掌握卡尔曼滤波在MCU上实现方法的开发者。工程将ADC采样数据作为观测输入结合LCD显示、定时器中断与卡尔曼预测/更新函数代码可系统理解滤波流程及嵌入式算法优化思路为机器人导航、飞行控制等实时估计场景提供实用参考。 做嵌入式开发久了你迟早会遇到一个让人挠头的问题传感器数据跳得厉害。用STM32F103做ADC采集电池电压、读MPU6050角度、或者处理红外测距模块的信号时那种“数据像心电图一样乱蹦”的场景相信每个做过实际项目的朋友都经历过。为了把数据弄平滑我最早用的是滑动平均滤波发现延迟大得离谱后来试过限幅滤波又会把真实的变化信号一起削掉最终在一本老书的角落里翻到了卡尔曼滤波这个名字花了一周时间啃完理论、调通代码才算真正把这个问题解决掉。这篇文章我想把在STM32F103上完整实现卡尔曼滤波算法的过程、代码、以及调参经验都整理出来。适合正在用F103做传感器采集、信号处理或者想把姿态解算、数据融合做得更顺滑的开发者参考。你也可以把它当成一份“能直接抄作业”的工程笔记来看代码部分我会给到可编译的完整实现。1. 卡尔曼滤波的适用边界与F103的资源评估先别急着上代码先说清楚一个更重要的问题卡尔曼滤波到底适不适合你的项目以及F103这颗老而弥坚的Cortex-M3内核跑不跑得动这套算法1.1 什么场景下该用卡尔曼滤波卡尔曼滤波本质上是一个面向线性系统的最优状态估计器。它适用于这么几类场景系统模型比较清晰能用状态方程描述比如惯性导航中的角度积分、定位中的位置速度估计传感器的测量噪声是有规律的服从近似高斯分布比如温漂小但随机噪声大的MEMS加速度计你需要从一堆带噪声的观测值中估计出“看不见”的内部状态比如从加速度信号中估算倾角对数据实时性要求较高不能容忍滑动窗口那种明显滞后的响应反过来如果你的系统是强非线性的比如大角度摆动下的姿态解算基础卡尔曼滤波的效果就明显不如扩展卡尔曼EKF或者互补滤波。这时候该用哪种心里要有数。另外卡尔曼滤波还有一个特质容易被人忽略它不只是“滤掉噪声”更是在“预测修正”的循环里不断逼近真实状态。这意味着它对突发的异常跳变有一定容忍能力不像限幅滤波那样一刀切。1.2 F103跑卡尔曼的硬件底子够不够STM32F103的主频是72MHz没有FPU只有单精度浮点运算需要软件模拟——很多朋友一看到这点就面露难色。实际上一维卡尔曼滤波也就是标量卡尔曼每一步只需要几十次乘法和加法浮点运算量非常小。我实测下来在72MHz主频下执行一次完整的标量卡尔曼更新大约耗时8~15微秒这在绝大多数传感器采集周期1kHz~100Hz下完全不构成压力比你读一次ADC还要快。如果你的项目里同时跑FreeRTOS和多个外设卡尔曼滤波的计算开销也只是毛毛雨。真正要关注的是内存一个标量卡尔曼滤波器只需要维护几个浮点变量占用的RAM也就几十字节这在F103动辄20KB起步的SRAM面前完全不值一提。我见过有人非要在F103上跑矩阵形式的卡尔曼滤波代码那才是真的自找麻烦。矩阵运算会引入大量for循环和临时数组不仅代码臃肿而且对栈空间不友好的F103来说很容易出问题。对于单变量状态估计老老实实用标量形式效率差一个数量级。1.3 与滑动平均、一阶低通滤波的对比滤波方式延迟表现对突变信号的响应计算开销适用场景滑动平均窗口越长延迟越大迟钝容易削峰极低缓慢变化的温度、液位一阶低通滞后与平滑度强相关有滞后极低简单降噪对延迟不敏感限幅滤波无延迟只会削掉过大跳变极低剔除明显异常尖峰卡尔曼滤波自动调节平均延迟较小可随噪声水平自动调整较低运动姿态、动态测量、精确估计从这个表能看出来卡尔曼滤波的优势不是响应最快而是它能够根据当前数据的可信度动态调整“信数据”还是“信模型”。这也是后面整定Q、R参数时的核心思想。2. 一维卡尔曼滤波公式的工程化理解卡尔曼滤波的公式到处都能查到但一堆矩阵符号很容易把初学者劝退。这里我用一个“称体重”的类比把五条公式讲明白。理解透了再看代码你会发现代码其实就是这五条公式的直译。2.1 状态预测你觉得自己的体重应该是多少假设你昨天称的体重是70.0kg那么今天如果没有暴饮暴食你对体重的“先验估计”就是70.0kg左右。在这个阶段你还会给自己的估计打一个“可信度分”——叫误差协方差P。如果你觉得昨天称完到今天变化不会太大这个可信度分就维持在一个比较小的值如果你觉得可能浮动很大P值就会相应调大一些。在均匀运动或角度积分这类场景中预测阶段还会用到速度或角速度等控制量比如有陀螺仪数据时角度预测就是“上一时刻角度 角速度*dt”。这部分在代码里对应的是状态方程。2.2 测量更新用秤的读数修正你的估计当你踩上体重秤获得一个实测值Z比如71.5kg。这个读数本身有噪声噪声大小用测量噪声R表示。这时候你手头有两个信息基于模型的预测值70.0以及带噪声的实测值71.5。到底该信哪个取决于两者的可信度。卡尔曼滤波给出的答案是找一个系数卡尔曼增益K让最终的估计值 预测值 K * (实测值 - 预测值)。K的大小由P和R共同决定如果P大模型不可信、R小测量很准增益K接近1最终结果会更偏向实测值如果P小模型很准、R大测量噪声大增益K接近0最终结果会更偏向预测值这个逻辑和日常生活中的判断完全一致不确定的时候就多信证据证据不靠谱的时候就多信经验。2.3 五条公式的标量直译在单变量标量情况下卡尔曼滤波的五条公式可以最直白地写成预测状态: x_est A * x_prev B * u 预测协方差: P_est A * A * P_prev Q 卡尔曼增益: K P_est / (P_est R) 状态更新: x_new x_est K * (measurement - x_est) 协方差更新: P_new (1 - K) * P_est注意看这些全是标量运算没有矩阵没有for循环。我在F103上用的就是这套公式的直译实现效果稳定代码也极好维护。2.4 搞清楚A、B、Q、R这几个字母到底是什么写代码之前一定要把每个符号的物理意义想明白不然后面调参完全是在瞎试A状态转移系数。如果按时间步长dt来积分角度A1如果你认为下一时刻状态等于当前状态加速更新A也可能是一个与dt相关的数。B控制输入系数。B*u表示控制量对状态的影响在角度估计里u对应陀螺仪的角速度B就是dt。Q过程噪声协方差描述模型本身的不确定度。这个值越大滤波器越“不信任预测”K值会自动增大。R测量噪声协方差描述传感器读数的噪声方差。这个值越大滤波器越“不信任测量”K值会自动减小。Q和R的比值直接决定了整个滤波器的“性格”。我在第4章会详细讲怎么调这两个值。3. STM32F103上可编译运行的完整代码实现代码我用标准外设库Standard Peripheral Library来写因为在F103的存量项目里标准库的使用比例依然相当高而且相比HAL库标准库的代码量更精简逻辑也更直观。如果你想移植到HAL库思路完全一样只需要把底层数据获取函数换一下。3.1 卡尔曼滤波器结构体定义typedef struct { float A; // 状态转移系数 float B; // 控制输入系数 float H; // 观测系数一般为1 float Q; // 过程噪声协方差 float R; // 测量噪声协方差 float P; // 误差协方差 float x; // 状态估计值 float u; // 控制输入没有则为0 }kf_t;这个结构体设计的关键在于所有滤波器的状态都封装在里面如果你的系统要同时对多个通道比如三轴加速度计的X/Y/Z进行滤波只需要定义多个结构体实例互不干扰。3.2 滤波器初始化函数void KF_Init(kf_t *kf, float init_x, float init_p, float q, float r) { kf-A 1.0f; kf-B 0.0f; kf-H 1.0f; kf-Q q; kf-R r; kf-P init_p; kf-x init_x; kf-u 0.0f; }初始化时要注意两点init_x可以用第一次读取的传感器值也可以直接给0滤波器运行一小段时间后会自动收敛。init_p不能设成0。如果你初始对状态完全不确信P设大一点比如100滤波器会快速从测量值中收敛如果设成0算法会认为自己“无所不知”K一直等于0滤波器就永远无法跟踪真实信号了。3.3 核心滤波函数float KF_Filter(kf_t *kf, float measurement) { float x_est, p_est, k_gain; // 1. 状态预测 x_est kf-A * kf-x kf-B * kf-u; // 2. 协方差预测 p_est kf-A * kf-A * kf-P kf-Q; // 3. 卡尔曼增益计算 k_gain p_est / (p_est kf-R); // 4. 状态更新 kf-x x_est k_gain * (measurement - kf-H * x_est); // 5. 协方差更新 kf-P (1.0f - k_gain) * p_est; return kf-x; }这段代码就是前面讲的五条公式的直译。每个周期调用一次KF_Filter传入当前的传感器读数函数返回滤波后的估计值。每调用一次结构体内的x和P都会自动更新作为下一轮迭代的状态。3.4 在ADC采集流程中的实际应用这里给一个完整的应用示例用F103的ADC连续采集电位器电压采样频率1kHz然后用卡尔曼滤波消除采样噪声。kf_t kf_voltage; int main(void) { uint16_t adc_raw; float voltage_raw, voltage_filtered; ADC_Init(); // 初始化ADC USART_Init(115200); // 初始化串口用于观察数据 // 初始化卡尔曼滤波器 // Q0.01, R0.25 这两个值需要根据实际噪声调整 KF_Init(kf_voltage, 0.0f, 1.0f, 0.01f, 0.25f); while (1) { adc_raw ADC_GetValue(); voltage_raw (float)adc_raw * 3.3f / 4095.0f; voltage_filtered KF_Filter(kf_voltage, voltage_raw); printf(raw:%.3f filtered:%.3f\r\n, voltage_raw, voltage_filtered); delay_ms(1); // 1kHz采样率 } }注意我在调用滤波函数时把ADC的原始值先换算成了电压值0~3.3V因为卡尔曼滤波对“量纲”是敏感的。如果你用0~4095的原始值那Q和R的取值范围就完全不一样了同理适用于其他场景。保持量纲一致后续调参才不会被无谓的数值差异干扰。3.5 在STM32F103内部晶振模式下的外围考量热词里提到了“让stm32f103工作在内部晶振模式下”这个和卡尔曼滤波有个隐蔽但关键的联系内部RC振荡器的精度在温度变化时误差可达1%~2%如果你用它做定时器触发ADC采样采样周期就会跟着漂移而卡尔曼滤波的预测步长dt如果没跟着实际时间更新滤波效果会明显下降。一个务实的处理方法是用内部晶振时把B系数中隐含的dt按1.01倍估算并同步调大一点Q值给模型留出不确定度。这样即使采样周期略有漂移滤波器也不会因为“模型失配”而表现失常。4. Q与R参数整定决定滤波曲线走向的核心调试环节很多朋友把卡尔曼滤波代码跑通之后发现滤波出来的曲线要么还是毛刺丛生要么被“削”得完全失去反应速度。问题几乎都出在Q和R的取值上。这一章我把自己调参的完整思路拆解出来。4.1 先测量你的传感器噪声方差在整定参数前先把传感器静止放在桌面上连续采集1000个点计算这些数据的标准差σ然后R就取σ²。这一步是让“测量噪声”这个参数有实际依据而不是拍脑袋。举个实际例子我测量一个MPU6050加速度计的静止输出标准差大约是0.02g那么R 0.0004。如果你的采样值波动更剧烈R就需要相应调大。4.2 Q值的调节策略Q值代表你对模型的信任程度。一个相对靠谱的经验是先让Q取一个相对小的值比如Q R / 100然后观察滤波曲线对真实变化的响应速度。如果滤波后曲线太“肉”跟不上实际信号变化就增大Q增益K会相应变大滤波器会更快“追随”测量值 如果滤波后曲线还有明显毛刺就减小Q滤波器会更依赖预测模型曲线更平滑。这里有一个非常重要的认知Q和R的比值决定滤波器的行为而不是绝对值。你用浮点运算数值范围本身不会造成溢出或者精度灾难所以不必纠结于Q是0.01还是0.0001关键是它与R之间相差多少个数量级。4.3 一个可复现的整定实例假设我要对F103内部温度传感器的读数做滤波静止状态下ADC读数跳动范围大约在±5个LSB换算成噪声方差R 25系统启动到稳定温度变化速率的模型不确定度较小Q先取0.25初始P给1初始x用第一次采样值我用串口把原始数据和滤波数据同时打印出来用串口示波器软件观察发现在开机预热阶段原始数据漂移大约20个LSB滤波曲线也能跟上但波动明显缩小到了±2个LSB左右。这个效果对于温度这种缓变信号已经够用了。4.4 自适应调整的进阶玩法在某些项目中传感器噪声不是恒定的比如运动状态下的加速度计噪声远大于静止状态。这种场景下动态调整R值能显著改善滤波效果float R_adaptive R_base alpha * variance_of_recent_measurements;当传感器数据突然剧烈波动时R自动变大滤波器会更多依赖模型预测避免被噪声带偏当数据恢复平稳R变小滤波器重新信任测量值。这种“自适应卡尔曼滤波”在姿态解算和导航应用中非常实用F103完全跑得动这个额外的方差计算。5. 实测中踩过的坑与对应的优化方案代码写出来只是开始实测中踩坑才是最耗时间的。这一章我把自己在F103上真正遇到过的问题、排查思路和最终解决方案都列出来希望能帮你少走弯路。5.1 滤波发散数据突然跳到离谱的值现象滤波运行一段时间后输出突然飞出正常范围甚至变成NaN。排查链路第一步检查除法。卡尔曼增益计算中p_est / (p_est R)如果R被误设成0且P_est也趋近0就会出现0/0的情况。确认R必须大于0。第二步检查结构体是否被意外清零。如果多个任务同时访问同一个滤波器实例出现竞争条件P和x会被外部改掉。加个临界区保护或者让滤波器只在单一任务中调用。第三步确认没有开启FPU却用了double类型。F103没有硬件FPUdouble运算会带来明显的性能损耗且可能触发堆栈溢出。全工程用float不要混用double。我实际遇到的场景是RTOS里两个任务都调用了同一个滤波器实例的初始化函数导致运行中被重新初始化P值被重置为0滤波器瞬间“失明”。最终用信号量把初始化保护起来解决。5.2 滤波后曲线的相位延迟不可容忍有次做舵机角度闭环控制卡尔曼滤波后的角度比实际角度慢了近20ms导致系统振荡。最初把R调得很小想让滤波曲线更平滑结果相位延迟严重。解决方法增大Q值降低R值让增益K自动变大。调整后滤波器的“惯性”变小相位延迟从20ms压缩到5ms以内毛刺也控制在可接受范围。记住卡尔曼滤波本质上是在“平滑度”和“响应速度”之间做权衡没有一劳永逸的参数只有适合你应用场景的参数。5.3 多通道滤波的代码组织优化F103做四轴飞行器时需要对陀螺仪三轴角速度、加速度计三轴分量分别滤波。如果每个轴都单独写一份滤波代码代码冗余会非常严重。用结构体数组就能优雅解决kf_t kf_gyro[3]; kf_t kf_accel[3]; for (int i 0; i 3; i) { KF_Init(kf_gyro[i], 0.0f, 1.0f, 0.05f, 0.001f); KF_Init(kf_accel[i], 0.0f, 1.0f, 0.05f, 0.0004f); } // 在采集循环中 gyro_filtered[i] KF_Filter(kf_gyro[i], gyro_raw[i]);这种写法的好处是后续如果要在某个轴上单独调整参数直接改对应的数组元素即可不影响其他轴的滤波逻辑。5.4 串口打印拖慢采样节奏调试卡尔曼滤波时很多人习惯每采样一次就用串口打印一次原始值和滤波值。在115200波特率下每打印一组浮点数需要大约1~2ms如果采样周期本身是1ms串口就成了瓶颈采样周期被凭空拉长。解决方案在代码里维护一个数据缓冲区每16个采样点集中通过一次串口输出或者用DMA配合空闲中断来发送。我用这个方式把有效采样率从实际不到500Hz准确恢复到了1kHz。5.5 卡尔曼滤波与PWMDMA控制的配合看到热词里有“PWM DMA PA1 PA3”结合滤波来聊一下。用DMA自动搬运PWM比较寄存器值可以生成平滑变化的波形而卡尔曼滤波负责平滑输入侧的传感器信号两者配合能让整个控制链路更干净。比如用F103输出PWM控制舵机占空比的设定值是传感器经过卡尔曼滤波后的结果。滤波后的信号不容易引起舵机频繁抖动PWM波形也更平稳。此时要注意卡尔曼滤波每次输出的时间点与PWM周期对齐否则占空比更新频率不稳定会导致舵机出现额外的抖动。5.6 停机模式下的滤波状态保持问题F103进入停机模式Stop Mode后SRAM内容在部分低功耗配置下会丢失取决于你是否开启备份域保持卡尔曼滤波器的结构体状态P、x如果被清零唤醒后滤波器需要重新收敛会有一个较长的过渡期。我的做法是在进入停机模式前把卡尔曼结构的x和P保存到备份寄存器中唤醒后重新加载避免重新收敛。备份寄存器在F103里足够存这两个float的32位拆分量操作不复杂但能省下不少调试时间。个人经验总结在F103上做卡尔曼滤波这件事难度不在代码本身而在于理解这套算法的“脾气”——它既不是万能的平滑器也不是越低延迟越好的滤波器它是一个会根据你给的参数不断评估“哪个信息更可靠”的决策器。把Q、R、P这三个参数和实际物理环境对应起来滤波效果自然就出来了。最后分享一个我常用的调参小技巧用串口同时打印原始值和滤波值在室温环境下用一个电吹风去吹传感器观察滤波曲线对温度突变和缓慢漂移的响应。这样能快速判断出当前参数是偏“信任模型”还是偏“信任测量”再根据你的项目需求往对应方向微调即可。本文还有配套的精品资源点击获取