3. 传感器噪声模拟：在仿真中注入IMU、GPS、磁力计噪声，验证滤波器鲁棒性

传感器噪声，说白了就是真实世界和理想模型之间的那层「毛玻璃」。你写的卡尔曼滤波器在纯数学仿真里跑得飞起，一接上真实传感器就发散，十有八九是噪声模型没建对。

我个人习惯，在联调验证阶段会先做一件事：把噪声主动灌进去，看滤波器扛不扛得住。这比任何理论推导都管用。

3.1 为什么要单独做噪声注入？

很多同学觉得，仿真里加个高斯白噪声不就完事了？嗯，这里有个坑——真实传感器的噪声远不止高斯白噪声这么简单。

我遇到过的情况是：某次飞控在实验室静态测试时姿态估计非常准，一上无人机就开始剧烈震荡。后来抓日志分析，发现是电机振动引入了IMU的有色噪声，而滤波器模型里只配了白噪声方差。说白了，你的滤波器「不认识」这种噪声，自然就扛不住。

核心观点：噪声注入不是为了「让仿真更真实」，而是为了主动暴露滤波器的薄弱环节。你注入的噪声类型越丰富，滤波器的鲁棒性边界就越清晰。

3.2 三类传感器的典型噪声模型

我们逐个来看。IMU、GPS、磁力计，它们的噪声特性完全不同，注入方式也得区别对待。

传感器 主要噪声类型 典型来源 仿真注入方式
IMU（加速度计/陀螺仪） 白噪声 + 随机游走 + 偏置稳定性 热噪声、振动、温度漂移 叠加高斯白噪声 + 一阶马尔可夫过程
GPS 白噪声 + 多径效应 + 低更新率抖动 卫星几何分布、大气延迟 叠加高斯白噪声 + 随机丢包 + 延迟模拟
磁力计 硬铁/软铁干扰 + 白噪声 机载电流、铁磁材料 叠加常值偏置 + 旋转相关噪声

你看，IMU的噪声最复杂，因为它有低频漂移。GPS的噪声则带有明显的时间相关性——不是每个时刻都独立。磁力计更麻烦，它的噪声和飞行器的姿态本身耦合在一起。

3.3 实战：在MATLAB/Simulink中注入IMU噪声

我直接给一段我常用的代码框架。这段代码模拟了IMU的「白噪声 + 随机游走」组合。

% IMU噪声注入示例
dt = 0.01;                % 采样周期 10ms
t = 0:dt:10;              % 仿真时长 10s

% 参数设置
gyro_noise_density = 0.01;    % 陀螺仪噪声密度 (rad/s/√Hz)
accel_noise_density = 0.05;   % 加速度计噪声密度 (m/s²/√Hz)
gyro_bias_instability = 0.001;% 陀螺仪偏置不稳定性 (rad/s)
accel_bias_instability = 0.01;% 加速度计偏置不稳定性 (m/s²)

% 生成白噪声
w_gyro = gyro_noise_density * randn(3, length(t)) / sqrt(dt);
w_accel = accel_noise_density * randn(3, length(t)) / sqrt(dt);

% 生成随机游走偏置（一阶马尔可夫）
tau = 100;  % 相关时间常数
b_gyro = zeros(3, length(t));
b_accel = zeros(3, length(t));
for k = 2:length(t)
    b_gyro(:,k) = b_gyro(:,k-1) * (1 - dt/tau) + ...
                  gyro_bias_instability * randn(3,1) * sqrt(2*dt/tau);
    b_accel(:,k) = b_accel(:,k-1) * (1 - dt/tau) + ...
                   accel_bias_instability * randn(3,1) * sqrt(2*dt/tau);
end

% 最终噪声 = 白噪声 + 偏置
gyro_noise = w_gyro + b_gyro;
accel_noise = w_accel + b_accel;

我的经验：随机游走的tau值很关键。设太小（比如1秒），偏置变化太快，滤波器反而容易跟踪；设太大（比如1000秒），偏置几乎不变，又测不出滤波器的抗漂移能力。我一般从100秒开始调，观察滤波器估计误差的稳态值。

3.4 GPS噪声注入：别忘了延迟和丢包

GPS和IMU最大的区别是什么？更新率低，而且有传输延迟。很多仿真只加白噪声，忽略了延迟，结果滤波器在仿真里表现完美，真机上一对时间戳就崩了。

我曾经踩过这个坑。仿真里GPS更新率设10Hz，延迟设0，滤波器收敛得漂漂亮亮。结果真机测试时GPS模块输出延迟200ms，滤波器直接发散。后来我学乖了，仿真里必须加延迟模拟。

% GPS噪声注入（含延迟）
gps_update_rate = 5;          % 5Hz
gps_delay = 0.2;              % 200ms延迟
gps_noise_std = [0.5; 0.5; 1.0]; % 位置噪声标准差 (m)

% 模拟延迟缓冲区
buffer_size = round(gps_delay / dt);
gps_buffer = zeros(3, buffer_size);

for k = 1:length(t)
    if mod(k, round(1/(gps_update_rate*dt))) == 0
        % 生成带噪声的GPS测量
        gps_meas = true_position(:,k) + gps_noise_std .* randn(3,1);
        % 存入缓冲区
        gps_buffer = [gps_buffer(:,2:end), gps_meas];
    end
    % 输出延迟后的测量
    gps_output(:,k) = gps_buffer(:,1);
end

注意：GPS延迟会导致滤波器中的测量更新和状态预测时间错位。如果你用的是标准卡尔曼滤波，必须做时间戳对齐，否则误差会累积。我建议在滤波器里维护一个「测量时间戳队列」，而不是直接用最新测量。

3.5 磁力计噪声：最难搞的噪声

磁力计的噪声注入，我个人觉得是最考验功力的。为什么？因为磁力计的误差和飞行器的姿态强相关——你转个方向，硬铁干扰的方向也跟着转。

简单加个常值偏置是不够的。我常用的方法是：

  1. 硬铁干扰：加一个常值偏置向量，模拟机载永磁体或电流产生的固定磁场。
  2. 软铁干扰：加一个与姿态相关的缩放矩阵，模拟铁磁材料对磁力线的扭曲。
  3. 白噪声：叠加高斯白噪声，模拟电子噪声。
% 磁力计噪声注入（含姿态相关干扰）
% 假设当前姿态四元数为 q
R = quat2rotm(q);  % 旋转矩阵

% 硬铁偏置 (常值)
hard_iron = [0.1; 0.05; -0.02];  % 单位: Gauss

% 软铁矩阵 (姿态相关)
soft_iron = [1.02, 0.01, 0.005;
             0.01, 0.98, -0.008;
             0.005, -0.008, 1.01];

% 真实地磁场 (假设已知)
mag_true = [0.3; 0.0; -0.5];  % 单位: Gauss

% 测量模型
mag_meas = soft_iron * (R * mag_true) + hard_iron + 0.02 * randn(3,1);

关键点：磁力计噪声注入的核心是区分「可校准部分」和「不可校准部分」。硬铁和软铁可以通过椭球拟合校准，但残差和随机噪声才是滤波器需要面对的。我一般先做一次椭球拟合校准，然后用校准后的残差作为噪声注入的基准。

3.6 如何验证滤波器鲁棒性？

噪声注入完了，怎么判断滤波器「扛住了」？我有一套自己的评判标准：

指标 计算方法 合格标准
姿态估计误差RMS sqrt(mean((估计值 - 真值)²)) < 2° (静态) / < 5° (动态)
位置估计误差RMS sqrt(mean((估计值 - 真值)²)) < 0.5m (GPS可用) / < 5m (GPS丢包)
滤波器一致性 NEES (归一化估计误差平方) 95%置信区间内
收敛时间 从初始化到误差进入稳态的时间 < 5s

这里我特别想强调NEES这个指标。很多同学只看误差大小，不看滤波器的协方差是否合理。NEES能告诉你：你的滤波器是「过于自信」（协方差太小）还是「过于保守」（协方差太大）。

避坑指南：我曾经遇到一个滤波器，姿态误差只有1°，看起来很好。但NEES值高达20，说明协方差严重低估了实际误差。这意味着滤波器对异常测量的「警惕性」不够，一旦遇到强干扰就会发散。所以，误差小不等于鲁棒性好。

3.7 本章知识体系总览

下面这张图总结了传感器噪声注入与滤波器鲁棒性验证的完整流程。你可以把它当作一个检查清单。

传感器噪声注入与滤波器鲁棒性验证流程 传感器噪声注入与滤波器鲁棒性验证流程 IMU 噪声注入 GPS 噪声注入 磁力计噪声注入 白噪声 + 随机游走 偏置不稳定性 一阶马尔可夫过程 白噪声 + 随机丢包 传输延迟模拟 低更新率抖动 硬铁 + 软铁干扰 姿态相关噪声 椭球拟合残差 卡尔曼滤波器 鲁棒性验证指标 姿态/位置误差RMS NEES一致性检验 收敛时间

这张图把整个流程串起来了。从左到右：先分别给IMU、GPS、磁力计注入符合其物理特性的噪声，然后让滤波器处理这些带噪声的数据，最后用RMS、NEES、收敛时间三个指标来评判滤波器的鲁棒性。

我个人觉得，这个流程里最容易忽略的是磁力计的姿态相关噪声和GPS的延迟模拟。很多公开的仿真框架只做白噪声注入，导致滤波器在仿真里表现极好，一上真机就露馅。嗯，你想想看，如果你的滤波器连这些「真实世界的基本操作」都扛不住，那它真的能飞吗？

← 上一章 📖 返回目录 下一章 →