2. 传感器基础：加速度计、陀螺仪、磁力计的工作原理与误差模型
做姿态估计，说白了就是跟三个传感器打交道：加速度计、陀螺仪、磁力计。这三兄弟各有各的脾气，摸透了它们，你的卡尔曼滤波器才能跑得稳。
我记得刚入行那会儿，总觉得传感器数据拿过来直接用就行。结果呢？飞控在天上转了几圈，姿态就飘到姥姥家去了。后来才明白——不了解传感器的误差模型，做出来的滤波都是空中楼阁。
2.1 加速度计：感知重力，但别太信它
加速度计测量的是比力，不是单纯的加速度。什么意思？就是它测的是物体本身的加速度减去重力加速度。静止的时候，它测到的就是重力加速度g，方向竖直向上。
我习惯把加速度计想象成一个弹簧秤。你把它放在桌上，它感受到的是地球在"拉"它。你推它一下，它感受到的是你的推力加上地球的拉力。嗯，就是这么回事。
核心原理： MEMS加速度计内部有一个质量块，通过检测质量块位移引起的电容变化来推算加速度。说白了就是——质量块动了，电容变了，电压变了，加速度就知道了。
加速度计在姿态估计中的角色很明确：提供俯仰角和横滚角的参考。因为重力方向是固定的，通过测量重力在三个轴上的分量，就能算出姿态。
但这里有个坑——加速度计对运动加速度特别敏感。你想想看，如果无人机在急转弯，加速度计测到的就不只是重力了，还有离心力。这时候用它算姿态，结果肯定不准。
避坑指南： 我曾经在一个项目中直接用加速度计算俯仰角，结果飞机一加速，姿态就乱跳。后来加了低通滤波，把高频运动加速度滤掉，才稳定下来。记住：加速度计只适合做长期参考，不适合做短期动态。
2.2 陀螺仪：角速度的忠实记录者
陀螺仪测量的是角速度，单位是度/秒或者弧度/秒。它不关心你平不平，只关心你转得快不快。
MEMS陀螺仪的原理是科里奥利效应。一个振动的质量块，当它旋转时，会受到一个垂直于振动方向的力。这个力的大小跟角速度成正比。测出这个力，角速度就知道了。
陀螺仪的优点很明显：响应快，动态性能好。你转一下，它立刻就能测出来。姿态估计中，我们通常用陀螺仪做短期积分，得到姿态变化。
但陀螺仪有个致命弱点——漂移。积分一次，误差累积一次。时间长了，姿态就不知道飘到哪里去了。
| 传感器 | 优点 | 缺点 |
|---|---|---|
| 加速度计 | 长期稳定，无漂移 | 对运动加速度敏感，噪声大 |
| 陀螺仪 | 动态响应快，短期精度高 | 有漂移，长期不可靠 |
| 磁力计 | 提供航向参考 | 易受干扰，需要校准 |
你看，加速度计和陀螺仪正好互补。一个长期稳但短期乱，一个短期准但长期飘。卡尔曼滤波的核心思想，就是把这两个互补的信息融合起来。
个人经验： 我建议在陀螺仪积分时，一定要考虑地球自转的影响。虽然对于消费级MEMS来说，地球自转的角速度（15度/小时）远小于陀螺仪的噪声，但在高精度应用中，这个误差不能忽略。
2.3 磁力计：航向的指南针，但别太依赖它
磁力计测量的是磁场强度。在地球上，地磁场的方向大致指向磁北。通过测量磁场在三个轴上的分量，就能算出航向角。
磁力计的原理是霍尔效应或者磁阻效应。简单说就是——磁场变了，材料的电阻变了，电压变了，磁场强度就知道了。
但磁力计的问题比加速度计还多。首先，地磁场很弱，很容易被周围的铁磁物质干扰。你手机里的扬声器、马达，甚至电路板上的电流，都会影响磁力计的读数。
其次，磁力计需要硬铁校准和软铁校准。硬铁干扰是固定的偏移，软铁干扰是方向相关的缩放。不做校准，航向角能偏十几度。
校准方法： 我常用的方法是让传感器在三维空间里转几圈，采集数据后拟合一个椭球。椭球的中心就是硬铁偏移，椭球的形状就是软铁缩放。把这个椭球校正成标准球体，数据就干净了。
还有一个问题——磁倾角。地磁场不是水平的，在北半球它会向下倾斜。如果不做倾斜补偿，航向角在俯仰或横滚时就会出错。
避坑指南： 我曾经在室内做磁力计测试，结果数据乱得一塌糊涂。后来才发现，楼里的钢筋结构把地磁场扭曲得不成样子。记住：磁力计在室内、地铁、高压线附近都不靠谱。室外空旷环境才是它的主场。
2.4 误差模型：传感器不完美，你得接受它
每个传感器都有误差。我习惯把误差分成两类：确定性误差和随机性误差。
确定性误差：
- 零偏（Bias）：传感器输出为零时，实际输出不为零
- 尺度因子（Scale Factor）：输入和输出之间的比例关系不准确
- 非正交性（Misalignment）：三个轴不是严格垂直的
随机性误差：
- 白噪声（White Noise）：高频随机波动
- 随机游走（Random Walk）：零偏随时间缓慢变化
- 闪烁噪声（Flicker Noise）：低频漂移
在卡尔曼滤波中，我们通常把零偏建模成状态变量，让滤波器自己去估计和修正。白噪声则建模成过程噪声或者测量噪声。
我个人的习惯是，先用艾伦方差分析（Allan Variance）来辨识传感器的噪声特性。这个方法能帮你搞清楚——你的传感器到底有多烂，哪些噪声占主导。
实用技巧： 做艾伦方差分析时，采集至少2小时的静态数据。然后画出log-log曲线，从曲线上读出角度随机游走（ARW）和速率随机游走（RRW）。这两个参数就是卡尔曼滤波中过程噪声协方差矩阵的输入。
2.5 传感器融合的直觉
好了，三个传感器都讲完了。你可能会问：怎么把它们融合起来？
我打个比方。加速度计和磁力计就像两个"参考老师"，它们告诉你"你现在大概是什么姿态"。陀螺仪就像"运动记录员"，它告诉你"你刚才怎么动的"。卡尔曼滤波就是那个"班主任"，它综合参考老师的意见和运动记录员的记录，给出一个最靠谱的姿态估计。
参考老师的话不能全信，因为她们有时候会走神（运动加速度干扰、磁干扰）。运动记录员的话也不能全信，因为他记性不好（漂移）。班主任的任务就是——谁靠谱就多信谁一点。
这就是卡尔曼滤波的本质：根据传感器的噪声特性，动态调整信任权重。
核心公式直觉： 卡尔曼增益K = 预测的不确定性 / (预测的不确定性 + 测量的不确定性)。当测量噪声大时，K变小，更相信预测（陀螺仪积分）。当预测噪声大时，K变大，更相信测量（加速度计/磁力计）。
嗯，传感器基础就讲到这里。下一节我们会把这些误差模型写成数学公式，然后塞进卡尔曼滤波器的框架里。到时候你会发现——原来那些看起来很复杂的矩阵，背后就是这么简单的道理。