4、传感器模型与噪声：IMU、GPS、气压计、磁力计的仿真模型，高斯噪声与随机游走噪声注入

传感器模型，说白了就是给真实硬件拍一张「数字快照」。你想想看，飞控算法在仿真里跑得再好，如果传感器数据是假的、干净的、没毛病的，那到了真机上一准儿翻车。我最早做仿真时，就吃过这个亏——仿真里悬停稳得像块石头，上了真机却抖得跟筛糠似的。后来才发现，问题出在传感器噪声模型上。

这一章，咱们就把 IMU、GPS、气压计、磁力计这四个核心传感器的仿真模型拆开揉碎。重点讲两件事：一是每种传感器的典型噪声特性，二是怎么在仿真里注入高斯噪声和随机游走噪声。

4.1 传感器噪声的数学基础

先聊点数学，别怕，很浅。传感器噪声通常分为两类：

• 高斯噪声（Gaussian Noise）：也叫白噪声，服从正态分布。特点是均值零、方差固定。说白了就是随机抖动，高频、无记忆。
• 随机游走噪声（Random Walk）：也叫布朗噪声。它是高斯噪声的积分结果，低频、有记忆。你想想看，陀螺仪的零偏漂移就是典型的随机游走。

在仿真里，我们通常这样建模：

// 高斯噪声
noise_gaussian = sigma * randn();

// 随机游走噪声（一阶马尔可夫过程）
noise_rw = noise_rw_prev + sigma_rw * randn() * dt;

嗯，这里要注意：sigma 是标准差，randn() 是标准正态分布随机数。dt 是仿真步长，千万别忘了乘它，否则随机游走的强度会跟步长耦合。

4.2 IMU 传感器模型

IMU 包含加速度计和陀螺仪。它的噪声模型是所有传感器里最复杂的，因为既有高斯噪声，又有随机游走。

4.2.1 加速度计模型

真实加速度计的输出 = 真实比力 + 高斯噪声 + 零偏 + 零偏随机游走 + 刻度因子误差 + 交轴耦合。但在仿真里，我们通常只保留前三项：

acc_meas = acc_true + acc_noise + acc_bias;
acc_noise = sigma_acc * randn();          // 高斯噪声
acc_bias  = acc_bias + sigma_acc_bias * randn() * dt;  // 随机游走

我在项目中遇到过一个问题：加速度计的高斯噪声标准差设得太小，结果仿真里的姿态估计精度看起来特别好，但一上真机就崩。后来我学乖了，噪声参数一律从芯片手册里查，查不到就按经验值给个偏大的数。

4.2.2 陀螺仪模型

陀螺仪的结构跟加速度计几乎一样：

gyro_meas = gyro_true + gyro_noise + gyro_bias;
gyro_noise = sigma_gyro * randn();
gyro_bias  = gyro_bias + sigma_gyro_bias * randn() * dt;

陀螺仪的随机游走比加速度计更致命。为什么？因为角速度积分一次就是角度，随机游走会让角度误差随时间线性增长。你想想看，悬停 10 秒和悬停 100 秒，陀螺仪的漂移量差了一个数量级。

4.3 GPS 传感器模型

GPS 的噪声特性跟 IMU 完全不同。它没有随机游走，但有时延、丢星和多路径效应。在仿真里，我们主要关注位置和速度的噪声。

4.3.1 位置噪声

GPS 位置噪声通常是高斯分布，但水平方向和高度方向的方差不一样：

gps_pos_meas = gps_pos_true + gps_pos_noise;
gps_pos_noise = [sigma_xy * randn(); sigma_xy * randn(); sigma_z * randn()];

水平精度一般在 1-3 米（单点定位），高度精度差一些，5-10 米。我习惯把 sigma_z 设成 sigma_xy 的 2-3 倍。

4.3.2 速度噪声

GPS 速度是通过多普勒频移算出来的，精度比位置高得多：

gps_vel_meas = gps_vel_true + gps_vel_noise;
gps_vel_noise = sigma_vel * randn();  // 通常 0.05-0.1 m/s

这里有个细节：GPS 速度噪声是时间相关的，因为多普勒测量本身有平滑。但在仿真里，我们通常把它当成白噪声处理，够用了。

4.4 气压计传感器模型

气压计测的是大气压强，换算成高度。它的噪声模型很简单，就是高斯噪声加一个低频漂移（温度引起）。

baro_meas = baro_height_true + baro_noise + baro_drift;
baro_noise = sigma_baro * randn();
baro_drift = baro_drift + sigma_baro_drift * randn() * dt;  // 温度漂移

气压计有个特点：短期噪声大，但长期漂移小。你想想看，几秒钟内气压计跳个半米很正常，但半小时内的平均高度却挺准。所以它特别适合跟 GPS 融合——GPS 短期准、长期漂，气压计正好互补。

4.5 磁力计传感器模型

磁力计测的是地磁场矢量。它的噪声模型除了高斯噪声，还要考虑硬铁干扰和软铁干扰。在仿真里，我们通常只加高斯噪声和硬铁偏置。

mag_meas = mag_true + mag_noise + mag_hard_iron;
mag_noise = sigma_mag * randn();          // 高斯噪声
mag_hard_iron = [bias_x; bias_y; bias_z]; // 硬铁偏置，常数

硬铁干扰是飞行器本身的磁场，比如电机电流、铁磁材料。在仿真里，我习惯把硬铁偏置设成地磁场幅度的 5%-10%。

磁力计还有一个坑：它的噪声不是各向同性的。水平方向受电机磁场影响大，垂直方向受地磁倾角影响大。我建议在仿真里给三个轴分别设不同的标准差。

4.6 传感器噪声注入的实现框架

说了这么多，咱们看看怎么在代码里把这些噪声模型组织起来。我习惯用一个统一的传感器噪声管理器：

class SensorNoiseModel {
public:
    void init(uint32_t seed) {
        rng_.seed(seed);
    }

    // 注入高斯噪声
    double addGaussianNoise(double sigma) {
        return sigma * rng_.randn();
    }

    // 注入随机游走噪声
    double addRandomWalk(double &state, double sigma, double dt) {
        state += sigma * rng_.randn() * dt;
        return state;
    }

    // IMU 噪声注入
    ImuData addImuNoise(const ImuData &true_data, double dt) {
        ImuData noisy = true_data;
        noisy.acc += addGaussianNoise(0.02);          // 加速度计高斯噪声
        noisy.gyro += addGaussianNoise(0.01);         // 陀螺仪高斯噪声
        noisy.acc += addRandomWalk(acc_bias_, 0.002, dt);  // 加速度计零偏漂移
        noisy.gyro += addRandomWalk(gyro_bias_, 0.001, dt); // 陀螺仪零偏漂移
        return noisy;
    }

private:
    std::mt19937 rng_;
    double acc_bias_ = 0.0;
    double gyro_bias_ = 0.0;
};

这个框架的好处是：所有噪声参数集中管理，方便调参。我每次做联调时，第一件事就是打开这个文件，把参数调成跟实际硬件一致。

4.7 传感器模型知识体系

下面这张图，把四种传感器的噪声模型和它们之间的关系画清楚了。你仔细看看，会发现一个规律：IMU 是唯一同时包含高斯噪声和随机游走的传感器，其他三种只有高斯噪声。为什么？因为 IMU 的零偏会随时间漂移，而 GPS、气压计、磁力计的误差主要是测量噪声和外部干扰。

4.8 联调时的噪声参数调优建议

最后，聊点实战经验。噪声参数怎么调？我一般分三步走：

1. 查手册：从传感器芯片手册里找到噪声密度（如 0.01°/s/√Hz），换算成仿真里的标准差。
2. 跑开环：不加控制器，只记录传感器输出，看噪声统计特性是否跟理论一致。
3. 闭环验证：加上控制器，看滤波器能否正确估计状态。如果估计误差跟理论协方差对不上，说明噪声参数设错了。

嗯，这里要注意：仿真里的噪声参数不是越精确越好。有时候故意把噪声设大一点，反而能检验控制器的鲁棒性。我习惯在最终验证时，把噪声参数调成手册值的 1.5 倍，给系统留点余量。