第二章：传感器模型——IMU、GPS、磁力计、气压计
各位同学，欢迎来到实战课的第二讲。
上一章我们聊了EKF的整体框架，说白了就是“预测+更新”的循环。但EKF再厉害，也得有“食材”才能做饭。这“食材”是什么？就是传感器数据。
今天咱们就掰开揉碎，把四个最常用的传感器模型讲透：IMU（加速度计+陀螺仪）、GPS、磁力计、气压计。
我个人习惯，在调EKF之前，一定会先把传感器模型在脑子里过一遍。为什么？因为模型不准，后面所有参数调优都是白费功夫。你想想看，你拿一把不准的尺子去量东西，量出来的结果能信吗？
2.1 IMU模型：加速度计与陀螺仪
IMU是EKF的“心脏”。它提供的是高频测量，通常100Hz到400Hz。但它的毛病也很明显——噪声大、有零偏、还会漂移。
2.1.1 加速度计模型
加速度计测量的是比力（Specific Force），也就是物体受到的合力减去重力。数学上可以写成：
a_m = a_true - g + b_a + n_a其中：
a_m：测量值a_true：真实加速度（不含重力）g：重力向量（注意符号！）b_a：加速度计零偏（随时间缓慢变化）n_a：高斯白噪声
关键点：加速度计不能直接用来算位置！因为它测的是“比力”，不是“加速度”。你得先把重力减掉，才能得到真实的运动加速度。
我在项目中遇到过一件事：有个同学直接把加速度计数据积分两次算位移，结果飞了不到10秒，位置误差就飘到几十米。嗯，这就是典型的“没搞清楚模型”的坑。
2.1.2 陀螺仪模型
陀螺仪测量的是角速度。模型类似：
ω_m = ω_true + b_g + n_gω_m：测量角速度ω_true：真实角速度b_g：陀螺仪零偏n_g：白噪声
注意：陀螺仪的零偏漂移比加速度计更严重。我曾经见过一个IMU，上电后前5分钟零偏变化了0.5度/秒。如果不做在线估计，姿态误差会像滚雪球一样越来越大。
为什么会这样？因为陀螺仪积分一次就是角度，零偏误差会随时间线性累积。你想想看，0.5度/秒的零偏，飞一分钟就是30度误差，这还得了？
2.1.3 IMU噪声参数
调EKF时，IMU的噪声协方差矩阵（Q矩阵）是最关键的参数之一。我一般这样设置：
| 参数 | 典型值（消费级IMU） | 说明 |
|---|---|---|
| 加速度计噪声密度 | 0.01 ~ 0.05 m/s²/√Hz | 越大表示加速度计越“吵” |
| 陀螺仪噪声密度 | 0.001 ~ 0.01 rad/s/√Hz | 影响姿态估计的平滑度 |
| 加速度计零偏随机游走 | 1e-6 ~ 1e-4 m/s²/√s | 控制零偏变化速度 |
| 陀螺仪零偏随机游走 | 1e-7 ~ 1e-5 rad/s/√s | 太小则零偏更新慢，太大则估计抖动 |
我的经验：刚开始调参时，先把噪声密度设大一点（比如0.05），让滤波器更信任预测。等基本稳定了，再慢慢减小，让测量起更多作用。别一上来就追求“最优”，容易炸机。
2.2 GPS模型
GPS是室外定位的“主力军”。但它有两个硬伤：更新频率低（通常5-10Hz）、精度受环境影响大。
GPS的测量模型很简单：
p_gps = p_true + n_gps其中 n_gps 是高斯噪声，标准差一般在1-5米（单点定位）或0.1-0.5米（RTK）。
2.2.1 GPS的“脾气”
- 水平精度好，垂直精度差：GPS的垂直误差通常是水平误差的1.5-2倍。所以EKF里，GPS对高度的约束要弱一些。
- 有延迟：GPS数据从卫星到飞控，通常有100-200ms的延迟。如果你不做时间对齐，EKF的更新就会“错位”。
- 多路径效应：在城市峡谷或树荫下，GPS信号会反射，导致位置跳变。
避坑指南：我曾经在调试一架无人机时，发现GPS位置突然跳了10米，结果EKF直接跟着跳，飞机差点翻掉。后来加了“残差检测”逻辑，发现GPS异常时自动降低权重，才解决问题。
2.2.2 GPS与IMU的融合策略
GPS更新慢，IMU更新快。EKF的做法是：
- IMU数据来了，做“预测步”（时间更新）
- GPS数据来了，做“更新步”（测量更新）
- 两次GPS之间，全靠IMU积分撑着
说白了，GPS负责“拉回来”，IMU负责“填中间”。
2.3 磁力计模型
磁力计用来测量磁场，帮助确定航向（偏航角）。但它是四个传感器里最“娇气”的。
模型长这样：
m_m = R * m_earth + b_hard + S * m_earth + n_mR：旋转矩阵（从机体到导航系）m_earth：当地地磁场向量b_hard：硬铁干扰（由机载铁磁材料引起）S：软铁干扰矩阵n_m：测量噪声
核心问题：磁力计最怕“干扰”。电机电流、大电流导线、甚至附近的金属结构，都会让测量值失真。
2.3.1 磁力计校准
我建议，每次换飞控或改变机架结构后，都做一次磁力计校准。校准方法：
- 把无人机在空中转几圈（绕所有轴）
- 记录所有方向的磁场测量值
- 拟合出一个椭球，算出硬铁和软铁参数
嗯，这里要注意：校准时的环境要和实际飞行环境尽量一致。你在室内校准，拿到室外飞，地磁场方向都变了，校准等于白做。
2.3.2 磁力计在EKF中的使用
磁力计通常只用来修正偏航角。因为俯仰和滚转有加速度计和陀螺仪就够了，磁力计对这两个轴的贡献很小，反而容易引入噪声。
我一般这样设置：
- 磁力计的测量噪声设得比较大（比如10-20度），防止它把姿态带偏
- 只在GPS信号好的时候启用磁力计更新（因为地磁场模型需要位置信息）
- 如果发现磁力计残差突然变大，立即降低权重或直接禁用
2.4 气压计模型
气压计用来测高度。原理很简单：气压随高度增加而降低。但实际用起来，坑不少。
模型：
p_baro = p_0 * exp(-h / H_scale) + n_barop_0：海平面气压h：高度H_scale：标高（约8430米）n_baro：噪声
2.4.1 气压计的“天敌”
气压计最怕两样东西：风和温度。
- 风：无人机飞行时，螺旋桨下洗气流会冲击气压计，导致高度读数上下跳动。我见过最夸张的，悬停时气压计高度波动超过2米。
- 温度：气压计芯片本身对温度敏感。从室内拿到室外，温度变化10度，高度读数可能漂移1-2米。
我的做法：给气压计加海绵罩，减少气流冲击。同时在EKF里把气压计的噪声设成动态的——悬停时噪声大，平飞时噪声小。这样能有效减少高度抖动。
2.4.2 气压计与GPS高度融合
GPS高度不准，气压计受气流影响。两者融合，取长补短：
- 气压计提供短时间内的相对高度变化（平滑但会漂）
- GPS提供长时间内的绝对高度参考（不准但无漂移）
- EKF把两者加权平均，得到最优高度估计
2.5 传感器模型知识体系总览
下面这张图，是我自己总结的传感器模型关系图。你看一眼，就能明白每个传感器在EKF里扮演什么角色：
2.6 本章小结
好了，四个传感器模型讲完了。我帮你捋一下重点：
- IMU：高频但会漂，零偏必须在线估计
- GPS：低频但绝对准确，注意延迟和异常跳变
- 磁力计：航向专用，校准是前提，干扰是常态
- 气压计：高度好帮手，但怕风怕温度
记住一句话：没有完美的传感器，只有聪明的融合算法。EKF的价值，就是把每个传感器的优点用起来，缺点补上去。
下一章，我们会把这些模型装进EKF的框架里，看看它们是怎么协同工作的。到时候我会手把手带你推导公式、写代码。