卡尔曼滤波这名字做机器人、做飞控、做惯性导航的人天天挂在嘴边但真被问到“它内部到底怎么一步步算的”能把五个公式默写出来的人其实不多。我见过太多人调MPU6050姿态解算代码是从网上抄的参数是拿经验值瞎凑的滤波效果一塌糊涂也不知道该动哪个量。这次我用Excel把卡尔曼滤波从预测到更新完整手推了一遍不写一行代码纯靠单元格公式复现整个姿态解算流程。每个中间变量怎么算、每步更新到底在做什么全部摊开在表格里看得一清二楚。这篇内容适合那些“代码能跑但原理模糊”的开发者也适合正在学卡尔曼滤波但被矩阵推导劝退的学生。看完你不仅能手动推完一次完整滤波还能学会怎么用Excel快速验证和改进自己的算法参数。1. 为什么用Excel手推卡尔曼滤波1.1 卡尔曼滤波不是什么玄学就是五步迭代卡尔曼滤波的本质一句话说就是用系统模型预测下一步状态再用带噪声的观测值去修正这个预测。它不要求你记住复杂的矩阵公式关键是要理解每一步在干什么。整个算法压缩到最小就是五个公式的循环使用系统模型和上一时刻的最优估计预测当前时刻的先验状态根据上一时刻的误差协方差和过程噪声计算先验误差协方差由先验误差协方差和观测噪声计算卡尔曼增益用当前观测值和卡尔曼增益把先验状态修正为后验状态也就是当前时刻的最优估计更新后验误差协方差为下一轮迭代做准备这五步从第一次接触的人视角看最难的是第三步和第五步涉及矩阵乘法和矩阵求逆。而用Excel手推正好能把这些矩阵运算拆成逐行逐列的标量计算让每一步的数值变化都直观可见。我之前带过一个实习生看了两天公式推导还是一头雾水后来我把这个过程做成Excel表格给他他顺着公式拖了十几行数据突然就“通了”。这就是把抽象矩阵变成具体数字的力量。1.2 姿态解算里卡尔曼滤波到底在融什么MPU6050姿态解算这个场景特别适合用来理解卡尔曼滤波因为它天然有两个来源不一致的观测加速度计和陀螺仪。加速度计在静止时能直接测出重力方向从而算出俯仰角和横滚角但动态下噪声特别大一震动输出就疯狂跳变陀螺仪短时间积分出来的角度非常平滑但有零漂时间一长积分误差累积角度会慢慢飘走。卡尔曼滤波在这里干的事就是“取长补短”短时间以陀螺仪积分为主因为它平滑但每次用加速度计的观测值来修正陀螺仪积分产生的漂移。这本质上是个数据融合问题而卡尔曼滤波给出了一个最优权重——也就是卡尔曼增益K。K大说明更相信观测值K小说明更相信模型预测。这些都是可以量化的而量化的过程放在Excel里等于是在“用肉眼观察滤波器的思考过程”。2. 手推前的准备状态方程、观测方程与参数设定2.1 MPU6050姿态解算里的状态量怎么选做俯仰角Pitch或横滚角Roll的单轴姿态解算时状态向量的选择直接影响整个模型的复杂度。最简洁实用的做法是选两个状态量角度angle陀螺仪零偏bias为什么要把零偏设成状态量这是很多初学者容易忽略的。陀螺仪逐次上电后的零偏不完全一样温度变化也会让它缓慢漂移。如果把bias当作状态量卡尔曼滤波会在运行过程中自动估计并修正它相当于给系统加了一个“自校准”机制。如果你不估bias积分漂移会持续累积最终结果一定发散。状态方程长这样angle_new angle_old (gyro - bias) * dt bias_new bias_oldgyro是陀螺仪角速度dt是采样周期。第二个方程说“零偏不变”是对系统行为的一种近似假设实际零偏会有随机游走这个“随机”程度就由过程噪声参数来控制。观测方程则很直接angle_meas angle vangle_meas是加速度计算出的角度v是观测噪声认为它服从零均值高斯分布。这个模型是一个线性模型矩阵形式为F [[1, -dt], [0, 1]]观测矩阵H [[1, 0]]。2.2 过程噪声和观测噪声的物理含义卡尔曼滤波有两个可调参数旋钮过程噪声协方差Q和观测噪声协方差R。它们分别是模型可信度和观测可信度的“反向度量”。Q越大表示你越不信任模型预测滤波器会更多地采信观测值表现是响应快但噪声大。R越大表示你越不信任观测值滤波器会更依赖模型积分表现是曲线平滑但有延迟。在姿态解算这个场景里两者的物理意义可以这样理解陀螺仪积分模型的主要误差来源是零偏随机游走所以Q里对应bias项也不能设为零否则滤波器永远不会修正bias估计。加速度计观测误差主要来自运动加速度的干扰静止时R可以设小一点运动剧烈时R要相应调大。在Excel推演中Q和R是单元格里的常数。调参时只改这几个格子整个滤波效果立刻变化。这种“改一个参数、拉一次图表”的即时反馈比在嵌入式代码里烧录调参要高效太多。初始误差协方差P0一般设为单位矩阵就行它代表对初始状态估计的不确定性迭代几轮后P会自动收敛到正常水平。2.3 Excel工作表的结构设计思路要把五步公式在Excel里落地第一步是设计清晰的工作表结构。我的方案是把参数区、数据区和计算区彻底分开互不干扰。参数区放在工作表顶部固定位置C1采样周期dt秒比如0.01C2过程噪声Q_angle对应角度随机游走强度C3过程噪声Q_bias对应零偏随机游走强度C4观测噪声R对应加速度计角度测量噪声计算区从第3行开始每一行代表一个采样时刻列布局依次为列含义说明A时间戳累加dt得到B陀螺仪角速度原始数据单位rad/sC加速度计角度原始观测值单位radD-E先验状态预测出的角度和零偏F-I先验协方差P矩阵的四个元素J-K卡尔曼增益K1和K2两个分量L-M后验状态最优估计结果N-Q后验协方差P矩阵更新后的四个元素R新息观测值和先验预测的差每一行就是一次完整的五步滤波迭代。看起来列数多但公式只写一行往下拖就全部搞定。这种结构的好处是某个时刻发生了什么、为什么角度输出会在那个位置跳一下全都能定位到具体单元格。3. 五步公式的Excel落地3.1 第一步先验状态预测先验状态预测是把上一时刻的后验状态代入状态方程算出这一步“按照模型应该是什么样子”。在Excel里假设上一时刻后验角度在L列、后验零偏在M列当前采样行的陀螺仪角速度在B列那么当前行的先验角度和先验零偏公式为D行 L上一行 (B当前行 - M上一行) * $C$1 E行 M上一行这里有三个细节值得注意。一是顺序问题公式用的是上一时刻的后验值而不是本时刻刚算出来的东西千万不能把L列改成D列自身否则就变成递归套娃结果一定发散。二是dt的引用要加绝对引用符号$C$1这样下拉填充公式时dt不会跟着变。三是先验零偏直接继承上一时刻的后验零偏这一步看起来简单但它决定了卡尔曼滤波“估零偏”的能力bias的更新完全靠第四步的新息来驱动。我在带人做这个实验的时候经常让学员先手工算三行数据再用Excel公式验证。手工算完第一行基本上就能理解“预测”和“更新”的区别了。3.2 第二步先验误差协方差预测先验协方差预测的作用是量化“模型预测到底有多不准确”。即使状态量本身没噪声预测也会引入不确定性因为模型本身不完美、输入有噪声。误差协方差P的更新公式是P_pri F * P_post * F^T Q。这里如果直接用矩阵公式对小白来说就是劝退。但展开成标量公式就变成了几个单元格的加减乘除。设上一时刻的后验协方差为NP11、OP12、PP21、QP22则当前行的先验协方差为F行 N上一行 - $C$1 * P上一行 - $C$1 * O上一行 $C$1 * $C$1 * Q上一行 $C$2 * $C$1 * $C$1 G行 O上一行 - $C$1 * Q上一行 H行 P上一行 - $C$1 * Q上一行 I行 Q上一行 $C$3 * $C$1 * $C$1可能有人会问F行的公式里为什么会多出Q_angle乘以dt平方这一项这是过程噪声从连续时间到离散时间的离散化结果。简单理解就是时间步长越大模型预测的不确定性就越大所以过程噪声项带上了dt的平方。这属于“知其所以然”层面的东西用过一次以后你就不会再对Q参数一头雾水了。3.3 第三步卡尔曼增益计算卡尔曼增益是卡尔曼滤波的“智能权重”它决定了观测值和模型预测值的信任比例。增益大就多信观测增益小就多信预测。在姿态解算这个场景里因为观测矩阵H [[1, 0]]增益公式可以化简为标量形式S F行 $C$4 J行 F行 / S K行 H行 / SS在卡尔曼滤波里叫做新息协方差是“先验不确定性 观测噪声”的总和。如果上一轮预测非常准P很小且观测噪声很小R很小那么S就小增益趋近1滤波结果几乎等于观测值反过来如果模型预测很准S就被P主导增益趋近0滤波结果几乎等于预测值。在Excel里这两个公式没有技术难度但值得停下来看几组数值变化。比如当R从0.01调到0.1时K1明显变小输出曲线会变“懒”滞后感立刻出现。我第一次在Excel里看到这个规律时特别有感触——原来那些号称“调好的飞控参数”本质就是在调这个比值。3.4 第四步后验状态更新后验状态更新是滤波器的“校正环节”用实际观测值来修正模型预测。先算新息Innovation再按增益比例修正状态R行 C当前行 - D当前行 L行 D当前行 J当前行 * R行 M行 E当前行 K当前行 * R行新息就是“观测值减去预测值”它代表了模型没能解释的那部分偏差。如果新息长期偏大且方向一致说明模型有系统误差如果新息在零附近随机波动说明滤波器工作正常。这一步逻辑很直观先算出误差再乘以信任比例把误差分摊到角度和零偏两个状态上。这时你能清楚看到零偏是怎么被估计出来的——如果角度持续被预测偏高新息是负的M行的零偏估计就会逐步调整反过来修正下一轮的积分结果。这种“状态变量内部互相制约”的关系看书上公式很难体会但在Excel里看几行数字就知道怎么回事了。3.5 第五步后验协方差更新后验协方差更新本质上是“把修正后的不确定度记入账本”为下一轮预测做准备。公式为P_post (I - K * H) * P_pri展开后N行 (1 - J当前行) * F当前行 O行 (1 - J当前行) * G当前行 P行 H当前行 - K当前行 * F当前行 Q行 I当前行 - K当前行 * G当前行这里有一个很重要的现象P矩阵的值在滤波器稳定后会收敛到某个固定值而不是持续减小到零。因为每轮预测都会注入新的过程噪声Q协方差不会无限缩小。如果发现P收敛到零说明Q被设成了0模型被完全信任观测逐渐失效滤波器就会“死掉”——这个问题我在下一章细说。到这里五步公式就全部落地了。在Excel里选中D3到R3往下拖到数据末尾整条卡尔曼滤波就完成了。3.6 图表检验与多组数据对比公式填充完成最后一步是画图。选择A列作为横轴L列后验角度和C列加速度计原始角度作为纵轴插入折线图对比两条曲线的跟随效果。再加一列D列先验角度的曲线就能同时看到预测和更新的差异。看图的重点有三个地方平滑度后验角度曲线应该比加速度计原始角度平滑很多噪声被明显抑制跟随性静止转运动时后验角度能不能快速跟上真实角度变化滞后多少零偏估计M列的零偏数值是否在一个合理范围内缓慢波动这时候调参就变成了“拖数据条、看曲线反应”的交互式操作。Q大一点响应快但曲线毛糙R大一点曲线丝滑但有延迟。调过几次以后你对卡尔曼滤波参数的理解会远超直接抄代码的开发者。4. 常见问题与调参实录4.1 滤波发散P矩阵为什么“死掉”了最典型的故障现象是前几行数据正常后面输出直接飘到天上或者卡在某个值不动完全跟随观测量。遇到这种情况先看F列的P11是不是在迅速变小甚至变成负数。P矩阵“死亡”的常见原因是过程噪声Q设置过小。Q太小模型被认为“绝对可靠”预测值权重无限大观测值被彻底忽略滤波结果等于陀螺仪积分——最后当然发散。另一个原因是初始P设置异常导致增益计算出现数值问题比如P初始设成0滤波器认为初始状态完全准确后续再也不会信任观测。我在Excel排查时习惯做一件事把R列新息拉出来看。如果新息长期在一个方向偏大说明滤波器确实“不听观测的话”。此时把Q_angle调大一个数量级比如从0.001调到0.01或者把R调小再观察曲线变化。这两个旋钮来回试几次基本能找到合理的平衡点。4.2 单位混用弧度制与角度制的经典翻车姿态解算里单位混用是最隐蔽的坑。MPU6050的陀螺仪原始数据是每秒多少度dps而加速度计算角度一般用radian。很多新手直接把两者放进同一个公式结果角度在一两秒内就冲到了几百。在Excel里做手推这个问题暴露得更明显。方案是把整个表格统一使用弧度制陀螺仪角速度除以57.2958换算成rad/s加速度计角度也统一用atan2函数输出弧度值。然后在表格顶部的参数区用一个大字号单元格标注“所有角度统一为弧度”防止自己下次打开表格时忘了。零偏的单位也要跟着转——陀螺仪零偏的单位是rad/s不是dps。我第一次做这个表的时候零偏单位没换算导致M列的bias估计比真实值大了57.3倍直到调参时发现零偏总在一个离谱的数值上跳才反应过来是单位问题。4.3 加速度计噪声大、陀螺仪零漂大该怎么设参数这是姿态解算调参的核心矛盾加速度计高频噪声大但无漂移陀螺仪平滑但有积分漂移。两者怎么权衡完全看应用场景。如果你做的是静态倾角传感器物体基本不动加速度计噪声是主要矛盾R可以设小、Q可以设大让滤波器尽量相信观测值输出会比较干净。如果你做的是动态机器人或四轴飞控运动加速度会严重干扰加速度计测倾角这时候应该把R调大、Q调小让滤波器在短时间内更依赖陀螺仪积分。代价是响应会变慢存在滞后。R值的确定经验上可以先从加速度计静止时的角度标准差来估算。比如静止采集1000次角度数据算标准差为0.01radR可以设为0.0001标准差的平方。Q值的确定相对主观先从一个小的经验值开始比如Q_angle0.001、Q_bias0.003然后看曲线动态调。记住一个原则拿出手机录一段实际运动轨迹对比滤波输出和真实姿态永远比凭空盯参数更靠谱。4.4 Excel操作里的几个坑用Excel做这件事有几个坑值得提前说。第一公式下拉填充前务必确认所有常量单元的引用都加了$绝对引用否则下拉一行dt就变成了下一行单元格的值整个滤波结果全歪。第二Excel默认显示的小数位数可能掩盖细微数值变化建议把计算区的单元格格式设为“科学计数”或保留6位小数否则无法观察P矩阵的收敛过程。第三网上常见的问题“Excel无法复制粘贴”很多时候是表格里有循环引用或者公式错误导致Excel进入保护模式先把错误提示解决再继续操作不要强行复制否则会复制一堆错误值进去。还有一个实用技巧如果数据量很大几百上千行不要一行一行拖公式选中计算区的前100行然后按CtrlD向下填充效率高很多。填充完之后检查一下最后几行的公式引用的行号是否正确避免填充偏移。我在做这套Excel推演的时候还顺手验证过卡尔曼滤波和互补滤波的等价性——当卡尔曼增益K收敛到一个常数时整个滤波器的行为与固定系数的互补滤波几乎一致。这个结论从公式看不直观但把J列和K列拖成图表一眼就能发现增益在几十轮之后就趋于稳定。这说明什么说明卡尔曼滤波比互补滤波多的核心能力就是自适应地调整权重、在线估计零偏。用Excel把这些都“看”出来以后再回去读那些矩阵推导你会觉得它们突然变得友好多了。最后再分享一个我个人的习惯做完这个Excel模板之后我把Q、R、初始P都做成了Excel的“名称管理器”定义名称这样公式里写的就是Q_angle、R_angle这种可读性强的名字而不是$C$2这种冷冰冰的单元格引用。改参数的时候去名称管理器里改一次所有公式同步更新。这个习惯看起来不起眼但当你一个月以后重新打开这个表格看到满屏的D3、F4引用就会知道命名有多重要了。