用Excel手推卡尔曼滤波：MPU6050姿态解算从黑盒到白盒

卡尔曼滤波这五个字劝退了不知道多少做嵌入式的人。我也算其中一个——几年前给自平衡小车调姿态解算翻过不少教程也抄过开源代码。滤波器跑起来确实能出曲线可陀螺仪漂移一大、或者我想让响应更快一点的时候我连该动哪个参数都不知道。后来我索性把整个卡尔曼滤波搬进Excel姿态解算的每个中间值占一个单元格陀螺仪数据做预测加速度计做校正一行一行往下拖。那种每步计算都看得见的感觉比看十遍公式推导都管用。这篇文章就把这套Excel手推过程完整拆开先用最简模型说清楚姿态解算里卡尔曼滤波到底在解决什么问题再把标准五步递推每一步的物理含义讲透然后带你从空表开始搭出整个滤波器最后给出Q、R参数的调节经验和从一维走向三维工程时的避坑提醒。适合正在调MPU6050但被矩阵和高斯分布劝退的朋友也适合那些代码能跑但心里没底、想彻底弄懂滤波原理再动手写代码的人。1. 为什么选择Excel从公式黑盒到递推白盒1.1 卡尔曼滤波真正劝退人的地方市面上讲卡尔曼滤波的资料大部分一上来就是高维高斯分布、协方差矩阵、状态转移矩阵、观测矩阵那一整套符号体系。我并不是说这些推导不重要而是说对于我只想给IMU数据写一个能用的姿态滤波器这类需求直接从矩阵公式入手门槛太高而且特别容易让人产生一个错觉好像理解了公式就等于会用了。实际情况恰恰相反。我见过不少朋友卡尔曼滤波的公式能默写出来但是一到调参就抓瞎Q和R该设多少为什么别人的代码里Q0.01能用我抄过来曲线就抖得不行协方差矩阵P到底在描述什么这些问题光看公式和库函数根本得不到答案。代码实现是另一个层面的黑盒。现在MEMS库、MPU6050库、飞控源码里到处都是封装好的卡尔曼滤波函数输入几个数组输出一个角度。你当然可以直接调用但一旦遇到传感器噪声特性变了、采样频率变了、或者你需要把滤波频段调得更激进你就只能到处搜参数而不是自己判断。我自己的体会是卡尔曼滤波最难啃的不是数学而是你从来没有真正亲眼见过一次完整的递推过程——每个中间量是多大、K是怎么一点点收敛的、P在更新前后差多少。这些数值直觉靠读是读不出来的。1.2 Excel如何把这层窗户纸捅破Excel这个工具妙就妙在它把所有计算摊开在单元格里。卡尔曼滤波五步递推每一步可以对应一列每个中间结果都能看得清清楚楚。你拖一次下拉填充就是完成一次时间步的递推你改一个Q值整列数据立刻重新计算配合折线图能看到滤波器行为的变化。用Excel手推还有几个好处是写代码替代不了的中间量彻底可见x_pred、P_pred、K、x_upd、P_upd各自独立成列哪个量在什么规律下变化一目了然。参数修改即时反馈Q、R放在固定单元格里改一个格子所有行重新算完比改代码重新编译快得多。天然适合验证算法逻辑Excel表格的行和列就是时间和状态拉公式的过程和单片机里for循环递推完全一致逻辑对不上立刻能发现。图表直观呈现滤波效果加速度计角度、陀螺仪积分、卡尔曼估计三条曲线放在一张图里滞后、噪声、跟随性看得清清楚楚。所以我一直觉得凡是准备在C代码里写卡尔曼滤波的人都应该先在Excel里把流程跑通。这个习惯帮我省掉的排查时间远超当初搭表花掉的时间。1.3 这篇文章的Excel示例适合谁跟练如果你是下面这几种情况跟着做一遍会有收获正在调MPU6050、做自平衡小车/机械臂/两轮平衡车被卡尔曼滤波参数折磨能抄代码但讲不清楚卡尔曼增益为什么是这个形式的嵌入式开发者给学生讲卡尔曼滤波原理想要一个从数值上看得见摸得着的教学工具刚接触惯性导航想从最简单的模型入手建立整体认识的初学者。整个例子不需要装任何额外软件一个能吃Excel的电脑就行。2. 先定好模型MPU6050姿态解算里我们要估计什么2.1 陀螺仪和加速度计一个会飘一个会抖MPU6050里有两类传感器用途完全不同。陀螺仪测量角速度加速度计测量比力。姿态解算的基本思路就是取长补短。陀螺仪的优点响应快、短时间内的增量非常准。缺点也很明显它有零偏而且角速度需要积分才能得到角度积分会把零偏和噪声一点点累积起来时间一长角度就飘了。举个具体的量级例子如果陀螺仪有0.5°/s的零偏没有被补偿跑一分钟积分出来的角度会偏30°。这个误差在平衡车上足够让系统直接倒掉。加速度计的优点能直接测出重力方向从而算出倾角。缺点同样突出它对振动和运动加速度非常敏感车辆一加速、电机一启动测到的重力方向就歪了。拿它直接算姿态数值会跳来跳去。一个会飘、一个会抖把它们融合起来就是典型的卡尔曼滤波应用场景。从信号处理的角度看陀螺仪提供的是高频可信、低频漂移的信号加速度计提供的是低频可信、高频噪声的信号两者在频域上正好互补。2.2 从三维降到一维为什么先拿横滚角练手完整姿态解算有三个角度横滚角roll、俯仰角pitch、偏航角yaw。但直接上三维模型状态向量一下就变成四元数加零偏好几维推导和实现难度非线性上升。所以我建议先用一维模型练手就拿横滚角roll举例。一维模型的物理意义非常直接我们想知道当前传感器的横滚角是多少。陀螺仪告诉我们角速度把角速度乘以时间间隔dt积分就能推断角度变化加速度计则通过atan2(acc_y, acc_z)之类的计算直接给一个带噪声的倾角观测值。整个系统的状态方程可以写成x_k x_{k-1} ω * dt w其中x表示横滚角ω是陀螺仪角速度dt是采样周期w是过程噪声。观测方程是z_k x_k v其中z是加速度计算出来的角度v是测量噪声。过程噪声w的方差记为Q测量噪声v的方差记为R。这个一维模型虽然简单但它的结构和高维卡尔曼滤波完全一致有一个状态量有一个带噪声的控制输入陀螺仪有一个带噪声的观测加速度计。把一维逻辑搞懂后面扩展到多维只是把标量换成矩阵。2.3 为什么不能直接把加速度计角度当输出可能有人会问既然加速度计直接能算出角度那还要卡尔曼滤波干什么这里面的原因有三个。第一加速度计测量值在动态环境下会被运动加速度污染。MPU6050在电机振动、急加速、急减速时加速度计读到的并不是重力分量而是重力加运动加速度的合力。这时候算出来的角度严重失真。第二即使没有运动干扰加速度计的角度也带有高频噪声。静止放置时你可能看到角度在±0.5°甚至±1°之间跳直接用这个值控制电机系统会抖得厉害。第三加速度计无法提供可靠的偏航角。绕重力轴的旋转不改变重力向量方向加速度计对yaw角天生不可观测。姿态解算要得到完整的三个角必须依赖陀螺仪积分而积分漂移又需要某种方式抑制——这就是卡尔曼滤波的本职工作。3. 五步递推每一步在干什么手算过程逐项拆解卡尔曼滤波的每次迭代可以拆成五个步骤前两步是预测后三步是校正。我把每一步的含义和计算方式展开来说公式都用最简的标量形式方便对照。3.1 第一步先验预测——用陀螺仪积分猜一个角度公式x_pred x_prev ω * dt这是整个递推里最符合直觉的一步上一刻的角度加上陀螺仪测到的角速度乘以时间增量就得到此刻的角度预测值。注意这里完全没用加速度计数据也就是说这一步只相信陀螺仪。如果陀螺仪有零偏那么x_pred就会逐渐携带这个零偏的影响。后面校正步骤会把方向找回来。这一步里x_prev是上一轮第五步算出来的后验估计值而不是x_pred。这个细节在Excel里非常重要很多人在搭建递推表时搞混了当前轮的预测和上一轮的更新结果导致公式循环引用或结果发散。3.2 第二步先验协方差——预测的置信度如何变化公式P_pred P_prev QP表示状态估计的方差单位是角度平方°²。P越大说明我们对当前估计越不确定。为什么预测会让不确定度变大因为陀螺仪测量有噪声积分过程本身也在累积误差Q就是用来描述这个过程中新增的不确定度的。如果Q设得很大意味着我们认为陀螺仪积分每步都在引入显著误差系统不得不更依赖测量如果Q设得很小意味着我们认为预测非常可靠滤波会倾向于少用测量值。在一维无控制输入模型里预测协方差就是P加上Q这么简单。在带陀螺零偏的二维模型里这一步会变成一个矩阵加法但含义完全一样状态方程里的过程噪声协方差被叠加到预测协方差上。3.3 第三步卡尔曼增益——权重到底怎么算出来的公式K P_pred / (P_pred R)卡尔曼增益K是0到1之间的数它决定了预测值和测量值各占多大权重。P_pred大、R小的时候测量值比较可信K接近1说明我们打算多听加速度计的反过来P_pred小、R大的时候预测值比较可信K接近0说明我们主要依靠陀螺仪积分的方向。这个形式其实可以从最优加权角度理解如果两个信息来源都有噪声那么最小化估计方差的做法就是按它们的方差倒数比例加权。KP_pred/(P_predR)正是这个权重的归一化结果。要注意K不是固定不变的它在递推过程中会逐渐变化并趋于稳定也就是所谓稳态增益。后面调参部分我会专门讲这个稳态值的意义。3.4 第四步后验更新——用加速度计测量纠正猜测公式x_upd x_pred K * (z - x_pred)括号里的z-x_pred叫做残差或创新项它表示测量值比预测值高/低了多少。卡尔曼增益K决定有多少残差被用来修正估计。如果K0.1就意味着每轮最多吸收10%的偏差所以滤波曲线对阶跃响应会有滞后这是正常现象。从形式上看这一步就是加权平均的变形K越接近1结果越接近测量值K越接近0结果越接近预测值。理解了这个结构后面看互补滤波和卡尔曼滤波的关系时会非常顺畅。3.5 第五步协方差更新——纠正之后有多确信公式P_upd (1 - K) * P_pred得到测量值之后我们对状态的了解应该更确定所以协方差要减小。乘上(1-K)这个因子K越大P下降得越多说明测量对不确定度的削减越强。这一步完成后新的P_upd就是下一轮第二步要用到的P_prev。五步递推闭环循环往复。值得留意的是在这个一维例子里只要Q和R固定P会逐渐收敛到一个稳定值而不是一路衰减到0。原因是第二步预测会不断加回Q第四五步更新又把它压下去最后二者达到动态平衡。这个平衡点就是系统的稳态精度它不由初始P0决定完全由Q和R决定。3.6 五步循环的整体画面把五步合在一起看卡尔曼滤波就是一个预测-校正的循环先用陀螺仪积分从上一轮的最优估计推进一步得到一个先验估计和对应的不确定度再计算卡尔曼增益决定校正步的权重然后用加速度计观测值修正预测得到本轮的最优估计并更新不确定度。类比开车导航陀螺仪积分就像车辆里程计短距离很准跑久了会漂加速度计角度就像GPS定位单点看有噪声但不会长期偏离。卡尔曼滤波干的活就是根据两者的实时不确定度动态决定该信谁多一点。这个画面一旦建立起来去读那些矩阵公式就不会再觉得隔着一层了。4. Excel里逐行复现从空表到完整滤波器的搭建过程4.1 表格结构与行列设计下面以横滚角估计为例搭一个完整的Excel递推表。建议新建一个工作表按下表的列顺序排列。列内容说明Astep时间步编号Bdt采样周期单位秒Cgyro_deg_s陀螺仪角速度单位°/sDacc_angle加速度计倾角单位°Ex_pred先验预测角度单位°FP_pred先验协方差单位°²GK卡尔曼增益无量纲Hx_upd后验估计角度单位°IP_upd后验协方差单位°²第1行留作表头。第2行放初始状态只需要填H2和I2H2 0初始角度估计I2 1初始协方差P0可以先设大一些表示不确定再找一个参数区我习惯放在K1、K2、K3K1 0.01过程噪声方差QK2 1测量噪声方差RK3 0.01采样周期dt这里Q取0.01主要是教学演示数值大一点能看到K的收敛过程真实工程参数后面会说怎么估。4.2 核心公式与单元格引用逻辑从第3行开始写公式也就是第一个递推周期。假设步进数据从第3行开始那么各列公式如下E3 H2 C3 * K3 F3 I2 K1 G3 F3 / (F3 K2) H3 E3 G3 * (D3 - E3) I3 (1 - G3) * F3写完之后把E3到I3往下拉到第N2行N是数据条数。每拉一行就相当于卡尔曼滤波完成一次预测-校正迭代。几个关键的引用细节E3必须引用H2也就是上一轮的后验估计。千万不能引用E2因为E2没有值而且逻辑上预测是基于上一轮更新后的最优估计做的。F3必须引用I2同理P的预测是基于上一轮更新后的协方差。B列dt和C列陀螺仪、D列加速度计是外部输入数据可以从真实采集的CSV粘贴进来也可以用公式生成模拟数据。Q、R、dt三个参数用绝对引用K$1、K$2、K$3这样下拉时不会偏移。数据量方面100Hz的MPU6050采样跑10秒就是1000行Excel毫无压力。你甚至不需要写任何VBA普通公式就能完成。4.3 模拟数据数值走查看着估计值逼近真实值为了让读者能对一遍答案我构造一组模拟数据。设定陀螺仪有0.5°/s的零偏加速度计输出前5个周期为0°从第6个周期开始阶跃到10°模拟真实角度突变Q0.01R1dt0.01s初始x0P1。stepz_acc(°)x_pred(°)P_pred(°²)Kx_upd(°)P_upd(°²)0----0.00001.000010.00.00501.01000.50250.00250.502520.00.00750.51250.33880.00500.338830.00.01000.34880.25860.00740.258640.00.01240.26860.21170.00980.211750.00.01480.22170.18150.01210.1815610.00.01710.19150.16071.62160.1607710.01.62660.17070.14582.84770.1458810.02.85270.15580.13483.81620.1348910.03.82120.14480.12654.60280.12651010.04.60780.13650.12015.25560.1201这组数据很值得盯一会儿。前5步加速度计一直说0°但陀螺仪带着0.5°/s零偏所以估计值并没有停在0而是缓慢漂到0.012°附近——因为卡尔曼滤波不完全相信零偏差的预测也不完全相信0.0°这个测量。第6步突然测量值变成10°估计值并没有一步跳到10°而是跳到1.62°后面每步都往上追。原因就是K只有0.16左右每轮只吸收约16%的残差所以滤波曲线呈现一条平滑上升的跟踪曲线而不是瞬时跳变。现实中不会有这么干净的阶跃但这个示例能直观展示卡尔曼滤波的两个特性抑制噪声静态时估计值平滑和动态滞后阶跃时估计跟不上。后续调参就是在这两个特性之间找平衡。4.4 新手最容易踩的三个Excel坑搭建过程中有几个问题经常出现提前说一下能省不少时间。第一个坑是循环引用。如果在E3里误写了H2而H2本身又被公式依赖Excel会提示循环引用。要避免这个问题必须保证上一轮的后验值放在第2行起始区当前轮中间值从第3行开始展开两者在行号上错开。第二个坑是绝对引用没写$。Q、R、dt这三个参数如果不加绝对引用下拉公式时会跟着行号偏移导致后面的Q变成0.011、0.012之类的值滤波结果完全乱掉。务必写成K$1、K$2、K$3或者干脆把参数区选中后按F4。第三个坑是单位混用。dt单位是秒陀螺仪单位是°/s两者相乘才是角度加速度计计算角度时不同文献用的轴定义不同正负号也容易搞反。我建议在Excel表格里加一个角度残差辅助列先画图看看z_acc和x_pred是否在同一个量级上再继续调参。5. 调参实战Q和R的工程直觉与观测方法5.1 先估测量噪声方差RR描述的是加速度计倾角观测的噪声强度。最简单的方法是让IMU静止放稳采集一段加速度计计算出的角度数据求标准差σ然后R就等于σ²。举个例子静止状态下加速度计倾角在±0.8°之间波动标准差大约0.4°~0.8°那么R就可以取0.16~0.64。如果你把MPU6050装在电机振动很厉害的结构上这个标准差会明显变大R应该相应调大告诉滤波器测量值没这么可信。需要注意动态环境下加速度计受到运动加速度干扰噪声不再是零均值白噪声而是有偏噪声。这种情况下即使统计R也未必准确工程上常用自适应策略在检测到强运动或大加速度时把R临时拉大让滤波器更依赖陀螺仪。Excel手推阶段先不用搞自适应但心里要有这根弦。5.2 再估过程噪声方差QQ描述的是预测模型的不可信程度。在一维角度模型里预测误差主要来自陀螺仪噪声和零偏。如果陀螺仪的角度随机游走噪声是σ_g单位是°/s那么经过dt积分后角度不确定度的方差贡献大约是(σ_g·dt)²。比如σ_g0.1°/sdt0.01s理论值就是(0.001)²1e-6这是个非常小的数。但实际建模时Q还要包含零偏变化、模型误差甚至时间步长不固定的影响所以直接取理论值往往会导致滤波器太“自信”、响应太慢。我的经验是先按理论值放大10~100倍作为初始试探值再用阶跃响应来微调。回到前面的示例Q0.01对这个模型其实偏大在真实静止场景里会产生比较明显的跟踪噪声。但用它来教学有个好处K的收敛过程看得很清楚。实际调参时应该用小得多的Q值起步。5.3 核心调参方向Q大还是R大调参时记住两句话Q相对于R越大滤波器越相信加速度计响应快但噪声大甚至出现明显抖动。Q相对于R越小滤波器越相信陀螺仪积分曲线平滑但滞后大阶跃响应跟踪慢。这在Excel里很好验证把Q从0.01改成0.0001R保持1重新画图看曲线是不是明显平滑了、阶跃响应是不是也明显变慢了。再把Q改大曲线会紧跟加速度计测量值噪声也一起进来了。实际调试流程建议先用阶跃测试信号或手动翻转IMU观察滤波输出跟上真实角度变化的速度再静态放置观察噪声幅度。如果滞后大增大Q或减小R如果抖动大减小Q或增大R。一次只动一个参数在Excel里对比前后曲线比盲调快很多。5.4 稳态增益与互补滤波的关系一维卡尔曼滤波在Q、R固定时经过一段时间递推后P会收敛到稳定值K也会收敛到稳态增益K_ss。以上面Q0.01、R1为例继续迭代到几百步P会稳定在0.095°²左右K收敛到约0.095。这个稳态值由方程P(PQ)·R/(PQR)决定代入可得P≈0.095K≈0.095。稳态时第四步公式变成x_upd x_pred K_ss * (z - x_pred)而x_pred x_prev ω·dt代入展开x_upd (1-K_ss) * (x_prev ω·dt) K_ss * z这个形式和互补滤波完全等价陀螺仪积分结果占(1-K_ss)的权重加速度计角度占K_ss的权重。也就是说一维卡尔曼滤波在稳态时就是一个自适应权重的互补滤波只不过权重的选择不是拍脑袋定的而是由Q、R的统计关系推导出来的。理解了这层关系调参时就有了参照系你调整Q/R本质上就是在调整互补滤波的截止频率。K_ss大相当于互补滤波更信任测量值高频噪声更容易进来K_ss小相当于更信任陀螺仪积分滞后更明显。6. 从一维走向工程扩展方向与避坑提醒6.1 把陀螺仪零偏加进状态向量一维模型有个明显缺陷它假设陀螺仪是准的或者零偏可以被忽略。实际上MPU6050的零偏通常有0.3°~1°/s而且随温度变化单纯靠增大Q去吸收零偏误差效果有限。工程上更标准的做法是状态向量从一维变成二维x [角度, 陀螺零偏]^T状态方程变为角度_k 角度_{k-1} (陀螺仪角速度 - 零偏) * dt 零偏_k 零偏_{k-1} 噪声也就是说每次递推时先用当前估计的零偏去修正陀螺仪读数同时认为零偏本身是缓变的随机游走噪声。因为零偏是间接可观测的——当加速度计连续多步说你偏了而这个偏差不能单靠当前角度解释时卡尔曼滤波就会把一部分偏差归因于零偏从而在线估计并补偿它。从一维扩展到二维五步结构完全不改变只是每个数变成了2x2矩阵。Excel中做这个扩展相对麻烦因为每步的矩阵乘法都要展开成四个标量公式但逻辑和一维完全一致。我建议大家先做熟一维表再用MATLAB/Octave的矩阵形式验证二维逻辑最后再落到C代码。6.2 三维姿态解算四元数与EKF的复杂度跃升真实姿态解算要同时估计roll、pitch、yaw或者用一个四元数表示姿态。这时候的难点不只是维度变高更在于状态方程变成非线性的——旋转矩阵/四元数的更新里出现三角函数和乘法标准卡尔曼滤波的线性高斯假设不再成立。工程上的选择通常是两类一类是扩展卡尔曼滤波EKF每次递推时把非线性模型在当前状态点处线性化计算雅可比矩阵另一类是Mahony/Madgwick互补滤波它们直接用梯度下降或PI校正来求解姿态计算量小、实现简单在很多飞控和四轴项目里表现足够好。没有哪个方案是绝对正确的取舍标准是算力是否充足姿态变化是否剧烈传感器噪声模型是否已知我的建议是用Excel手推一维模型建立数值直觉但不要把卡尔曼滤波当作所有姿态问题的万能答案。真正到了三维先评估需求再决定用EKF还是互补滤波。6.3 从Excel到单片机几个必须处理好的工程细节在Excel里跑通逻辑后写C代码只是翻译工作。但有四个细节容易在移植时翻车提前说清楚。第一个是单位一致。陀螺仪原始输出是ADC值要先除以灵敏度换算成°/s或rad/s。如果后面要用四元数建议全程用弧度制避免在°和rad之间来回转换。Excel示例里用的是°嵌入式里统一用rad更常见。第二个是采样周期要稳定。卡尔曼滤波的公式默认dt是固定值如果你在主循环里用delay凑时间实际dt忽大忽小预测步的角度增量就不准。最好用定时器中断或者RTOS里的固定周期任务保证每次递推间隔一致。第三个是浮点计算。现在多数MCU带FPU用float就行尽量不要用double在无FPU的芯片上硬算速度和内存都会吃紧。矩阵运算的中间数组要小心内存碎片能静态分配就静态分配。第四个是全球坐标系对齐。MPU6050安装方向不同加速度计和陀螺仪的轴定义就可能不一致。很多滤波异常不是算法问题而是陀螺仪绕X轴的角速度被当成绕Y轴参与计算这类低级错误。移植后第一步先静止验证零速时滤波输出应该稳定且接近真实倾角再上动态测试。关于陀螺仪零偏还有一个很实用的预处理技巧上电后先让设备静止2~3秒取陀螺仪输出均值作为初始零偏从后续数据里减掉。这虽然不能替代卡尔曼滤波对零偏的在线估计但可以把起始阶段的零偏误差降低两个数量级让滤波器启动时就有不错的估计。个人实际体会把Excel手推表做一遍再写代码最直观的变化是以前调参靠蒙现在我能在一开始就估出大概的Q/R量级然后在表格里把参数扫一遍再上板以前看不懂网上代码里那些P矩阵更新的细节现在能顺着五步结构逐行对上了。这个习惯让我后来做更复杂的惯性导航项目时省了很多调试时间。如果你正准备调MPU6050建议先花一个晚上把这套Excel表拉出来拖一拖下拉填充把Q改大改小各看一次曲线——这种数值上的手感是任何公式推导都给不了的。