4. 传感器仿真与数据生成：IMU、GPS、磁力计与噪声建模
各位同学，欢迎来到传感器仿真这一章。
说实话，飞控算法里最让我头疼的，不是控制律设计，而是传感器数据。你想想看，算法写得再漂亮，传感器一抖、一漂、一丢星，全白搭。所以这一章，咱们把传感器仿真这件事彻底聊透。
4.1 IMU仿真模型：加速度计与陀螺仪
IMU是飞控的“内感受器”。它告诉你：我现在的加速度是多少？角速度是多少？
但IMU有个毛病——它测的不是理想值。加速度计测的是比力，不是纯重力。陀螺仪测的是角速度，但会漂。我刚开始做仿真时，直接拿理想值往里怼，结果算法在仿真里飞得挺好，一上真机就炸。嗯，从那以后，我再也不敢忽略IMU的测量模型了。
加速度计仿真模型
真实加速度计的输出，可以写成：
a_meas = a_true + g + b_a + n_a其中：
a_true：真实加速度（机体坐标系）g：重力加速度在机体坐标系下的投影b_a：加速度计零偏（随时间缓慢变化）n_a：高斯白噪声
陀螺仪仿真模型
陀螺仪的输出类似：
w_meas = w_true + b_g + n_g其中：
w_true：真实角速度b_g：陀螺仪零偏n_g：高斯白噪声
4.2 GPS仿真模型
GPS是飞控的“外感受器”。它告诉你：我在哪？我飞多快？
但GPS有延迟、有丢星、有多路径效应。我见过一个项目，仿真里GPS数据完美无缺，结果真机测试时，GPS一丢星，无人机直接飘走。所以，GPS仿真必须包含这些“坏情况”。
GPS位置仿真
pos_gps = pos_true + n_pos + bias_pos其中：
n_pos：位置噪声（一般1-3米标准差）bias_pos：缓慢变化的定位偏差
GPS速度仿真
vel_gps = vel_true + n_vel速度噪声一般比位置噪声小，约0.1-0.3 m/s。
4.3 磁力计仿真模型
磁力计用来测航向。但它特别娇气——周围有铁磁物质、电机电流、甚至地磁异常都会干扰它。
我曾经在测试时发现，无人机一靠近金属护栏，航向就偏了20度。查了半天，原来是磁力计被干扰了。所以，磁力计仿真必须包含硬铁和软铁干扰。
磁力计仿真模型
mag_meas = R * mag_earth + b_hard + S_soft * R * mag_earth + n_mag其中：
R：从导航系到机体系的旋转矩阵mag_earth：当地地磁场矢量b_hard：硬铁干扰（恒定偏置）S_soft：软铁干扰（比例矩阵）n_mag：测量噪声
4.4 传感器噪声与误差建模
噪声建模，说白了就是让仿真数据“像真的”。我见过很多同学直接加高斯白噪声，但真实传感器的噪声远不止这些。
常见的噪声类型：
| 噪声类型 | 来源 | 仿真方法 |
|---|---|---|
| 高斯白噪声 | 电子热噪声 | np.random.normal() |
| 随机游走 | 积分漂移 | 对白噪声积分 |
| 闪烁噪声 | 半导体缺陷 | 1/f噪声生成 |
| 量化噪声 | ADC量化 | 四舍五入到固定位数 |
误差建模的“避坑指南”：
- 我曾经在仿真里只加白噪声，结果滤波器收敛得特别快。一上真机，收敛时间翻了三倍。后来才发现，我忘了加随机游走。
- 量化噪声容易被忽略。但如果你用16位ADC，量化步长约0.3mg，这个误差在低动态场景下不可忽视。
- 温度漂移：我习惯在仿真中加入温度循环，让零偏随温度变化。这样能检验算法对温度变化的鲁棒性。
4.5 传感器数据融合仿真
数据融合，就是把多个传感器的数据“揉”在一起，得到更准确的状态估计。最经典的方法就是扩展卡尔曼滤波（EKF）。
EKF融合的基本流程：
- 预测步：用IMU数据预测状态（位置、速度、姿态）
- 更新步：用GPS和磁力计数据修正预测结果
- 时间对齐：处理不同传感器的延迟和频率差异
下面是一个简化的EKF融合代码框架：
# 预测步（IMU驱动）
def predict(imu_data, dt):
# 用陀螺仪更新姿态
# 用加速度计更新速度
# 更新协方差矩阵
pass
# 更新步（GPS/磁力计）
def update(measurement, H, R):
# 计算卡尔曼增益
# 更新状态估计
# 更新协方差矩阵
pass数据融合仿真还有一个关键点：一致性检验。我习惯在仿真中故意加入传感器故障（比如GPS突然跳变），看滤波器能不能检测并隔离故障。这个能力在实际飞行中比精度更重要。
本章知识体系
下面这张图，是我自己总结的传感器仿真与数据融合的知识结构。你可以把它当作一个“地图”，随时回来对照。
好了，这一章的内容就到这里。传感器仿真这件事，说难不难，说简单也不简单。关键是要把“真实世界的坏脾气”加进去——噪声、漂移、延迟、干扰。只有这样，你的飞控算法才能在仿真里“挨过打”，上了真机才能“扛得住”。