我有一个小玩具，有一块小屏幕，里面有一个立方体，会随着你的旋转而旋转。无论你把它怎么抛、怎么转，它最后总能稳稳地立在一个角上。看起来像魔术，但拆开来看，核心就是一块指甲盖大小的电路板，上面集成了一个六轴惯性测量单元——三轴加速度计和三轴陀螺仪。
我想起十年前刚接触无人机时，第一次用MPU6050模块的场景。当时只知道它能输出加速度和角速度，但对着满屏跳动的原始数据，完全不知道这意味着什么。后来才明白，让这个小立方体保持平衡的秘密，不在于传感器本身有多精密，而在于如何从这些看似杂乱的数据中，提取出真正有用的特征，再把它们巧妙地融合在一起。
这其实是个极好的切入点，用来理解IMU特征提取和数据融合的基本逻辑。
两个传感器，两个世界
先看加速度计和陀螺仪各自给了我们什么信息。
加速度计测量的是比力，也就是物体所受的线性加速度减去重力。当立方体静止时，它输出的就是重力在三个轴上的分量。通过这个分量，我们就能算出立方体相对于地面的倾斜角度。这种计算很直接，三角函数一算就出来了，但它有个致命缺陷：对振动极其敏感。你只要轻轻碰一下桌子，加速度计的输出就会剧烈跳动，因为它无法区分这是重力引起的，还是运动加速度引起的。
陀螺仪则完全不同，它测量的是绕各轴旋转的角速度。把角速度对时间积分，就能得到旋转的角度变化。陀螺仪的最大优势在于短时间内的动态响应特别好，你快速转动的每一个细节它都能捕捉到，完全不受线性加速度的影响。但它也有自己的问题——漂移。哪怕立方体静止不动，微小的零偏误差在积分过程中会不断累积，几秒钟后算出的角度就歪得离谱。
于是有意思的局面出现了：加速度计在静态下很准但动态下不可靠，陀螺仪在动态下灵敏但长时间会漂移。单独用哪一个都不行，但把它们放在一起，恰恰能互相弥补。加速度计提供绝对的“重力向下”参考，修正陀螺仪的长期漂移；陀螺仪提供高频的动态信息，滤掉加速度计对振动的过度反应。
互补滤波：最优雅的“和稀泥”
怎么把它们捏在一起，就是数据融合要解决的问题。
最直观也最经典的方案是互补滤波器。它的思路简单到近乎朴素：加速度计的噪声主要集中在高频，陀螺仪的漂移主要体现在低频，那我们就让加速度计的低频部分和陀螺仪的高频部分共同起作用。换句话说，角度估计主要由陀螺仪积分得到，但每隔一段时间，用加速度计的测量结果去轻轻地“拉”它一把，让它别偏得太远。
写成公式就是：角度 = 0.98 × (上一时刻角度 + 陀螺仪×dt) + 0.02 × 加速度计角度。
这个0.98和0.02的分配比例，本质上是在定义两个传感器的“信任边界”。系数不是随便选的——它等于截止频率的映射。系数偏向陀螺仪，就是更相信瞬时运动，适合高动态场景；偏向加速度计，姿态就更稳，但响应会变迟钝。调这个参数的时候，你实际上是在做一个判断：当前的场景，我是更怕漂移，还是更怕振动。
我第一次在自制的平衡小车上跑通互补滤波时，看到串口绘图器里那条平滑的角度曲线，有种说不出的快感。因为它足够简单，简单到让你能看穿数据融合的本质——让两个不完美的传感器，通过互相对照、互相矫正，组合出一个比它们任何一个都更接近真值的估计。
维度提升：当我们加入第三维
但立方体的平衡显然不只涉及一个维度的倾斜。它在三维空间中旋转，事情立刻变得复杂得多。
这里的核心难点在于旋转的不可交换性。你先把立方体绕X轴转90度再绕Y轴转90度，和先绕Y轴转90度再绕X轴转90度，最后得到的姿态是完全不同的。这意味着角度不能简单加减，整个计算必须在三维旋转群——也就是四元数或者旋转矩阵的框架下进行。
于是互补滤波器的三维版本登场了，也就是Mahony滤波器。它的核心逻辑和一维版本完全一致：用陀螺仪的数据递推姿态四元数，然后利用加速度计的测量值计算出一个“误差四元数”，这个误差代表了当前估计的重力方向与实测重力方向之间的偏差。最后用这个误差去修正陀螺仪的漂移，形成一个闭环。
这里有一个很妙的细节：加速度计只能提供两个自由度的参考——俯仰和横滚，它不知道你绕着重力方向转了多少度，也就是偏航角。所以这个修正只作用于倾斜方向，对偏航角毫无影响。这就要求我们在没有其他传感器的情况下，必须接受偏航角会逐渐漂移的事实。
立方体的设计者显然也意识到了这一点。小立方体之所以每次都能稳稳立住，正是因为它的目标状态——立在一个角上——只需要俯仰和横滚的控制，完全不需要偏航信息。这是硬件的限制催生出的巧妙应用，只利用自己能确定的自由度，不去纠结无法确定的那个。
卡尔曼滤波：另一种思维范式
互补滤波是频域思维的产物，卡尔曼滤波则代表另一种范式——概率思维。
它把整个过程建模成一个“预测-更新”的循环。预测阶段，根据陀螺仪数据和运动模型推算出下一时刻的姿态，同时给出这个预测的不确定性有多大。更新阶段，拿到加速度计的测量值后，比较测量值与预测值之间的差距，根据各自的噪声协方差矩阵，算出一个“最优加权”——这就是卡尔曼增益。最终估计等于是把预测值和测量值按可信度重新加权平均。
在立方体这个场景下，运算量较低的Mahony滤波器已经足够好了。但如果你要做的是一个高速穿越机，在急转弯时还要同时融合GPS、磁力计、气压计等多个传感器的数据，卡尔曼滤波的框架优势就体现出来了。它天然支持多传感器异步融合，能够最优地处理每一个信息源的不确定性。
不过有意思的是，无论互补滤波还是卡尔曼滤波，它们的内核高度一致。调参时你做的其实是同一件事——告诉算法：我有多相信这个传感器，有多相信那个模型。方法论的差异终究会落在应用场景的抉择上。没有绝对更优的算法，只有更匹配问题的方案。
特征提取的真正含义
说回特征提取。前面一直在讲如何从原始数据中估算姿态角度，这本身就是一种特征提取——从加速度和角速度的高频采样序列中，提取出一个物理意义明确的、低频变化的姿态状态。
但角度远不是唯一的特征。在更广泛的应用中，IMU特征提取可以得出运动强度、步态周期、振动频谱、静止状态检测，甚至结合机器学习提取出特定动作模式的深层特征。比如手机里的计步器，用的不是角度，而是加速度信号的周期性峰谷特征。无人机判断是否降落，需要识别出触地瞬间特有的高频振动模式。
小立方体最打动我的地方就在这里——它只用最基础的角度特征，配合一套巧妙的控制逻辑，就创造出了一个让人忍不住反复把玩的物件。它没有GPS，没有摄像头，甚至连一个磁力计都没有，靠着加速度计和陀螺仪这两个加起来不到十块钱的传感器，完成了一个看似不可能的任务。
这让我想到一个更宏观的隐喻。我们每天面对的信息，何尝不像IMU的原始数据一样充满噪声与漂移？短期的直觉像陀螺仪，灵敏却容易累积偏差；长期的反思像加速度计，稳重却容易被日常的琐碎干扰。做出好的判断，本质上也是在执行一种数据融合——让快速反应的直觉与缓慢积累的经验互相校准。
下次再看到那个小立方体稳稳地立在桌角时，我也许会想到：它不过是在用最朴素的方式，反复回答那个我们都必须面对的问题——在这个充满噪声的世界里，如何找到真正的“垂直”。
