3. IMU误差模型
在IMU预积分与因子图融合定位中，IMU误差模型决定了预积分协方差传播是否可信、零偏随机游走节点是否可观测、以及融合结果能否在长时间内保持稳定。如果只把IMU当作“理想角速度和加速度传感器”，预积分会在几十秒内漂移到不可用。本节从工程建模角度出发，把零偏、噪声、尺度因子、非正交误差四类误差拆开，并给出艾伦方差分析的具体操作流程。
一、核心原理：IMU原始观测的完整误差模型
对于MEMS IMU，真实角速度 ω 和比力 a 与测量值之间的关系可写成如下形式：
ω_meas = T_g · K_g · (ω_true + b_g + n_g)
a_meas = T_a · K_a · (a_true + b_a + n_a)
其中：
T_g, T_a 非正交/轴间耦合矩阵（上三角，对角为1）
K_g, K_a 尺度因子矩阵（对角，接近单位阵）
b_g, b_a 零偏（bias），随时间缓慢随机游走
n_g, n_a 白噪声，服从 N(0, σ²)在因子图中，通常把尺度因子和非正交误差视为标定后固定的系统误差，把零偏建模为随机游走状态节点，把白噪声建模为预积分协方差传播的输入。三者处理方式不同，不能混为一谈。
二、四类误差的建模方法
- 零偏 b(t)：建模为随机游走 ḃ = n_b，离散化为 b_{k+1} = b_k + n_b·Δt，噪声密度由艾伦方差零偏不稳定性系数给出。在因子图中作为独立节点，与预积分因子、GNSS因子、回环因子相连。
- 白噪声 n(t)：建模为连续高斯白噪声，离散化后方差为 σ²/Δt。它直接进入预积分噪声传播矩阵，决定预积分因子的信息矩阵。
- 尺度因子 s：建模为对角矩阵 K = diag(s_x, s_y, s_z)，通常在出厂或离线标定中估计，融合阶段作为常量修正测量值。
- 非正交误差 T：建模为上三角矩阵，描述三轴不正交带来的耦合。与尺度因子合并为 T·K，可用六位置法或椭球拟合法标定。
三、艾伦方差分析实战流程
艾伦方差（Allan Variance）是分离IMU噪声类型的标准工具。其核心思想是：对静止数据按不同时间簇长 τ 求平均，再计算相邻簇平均值的方差，得到 σ²(τ) 随 τ 变化的曲线。不同噪声在双对数坐标下呈现不同斜率。
- 采集静止数据：IMU水平静置2～4小时，采样率200Hz，环境温度稳定。
- 去除明显异常段和温漂剧烈段，保留连续有效数据。
- 按簇长 τ = m·Δt 分组，计算每组的平均值序列。
- 计算相邻平均值的差分方差，得到艾伦方差 σ²(τ)。
- 在 log-log 图上拟合斜率：-1/2 对应白噪声，0 对应零偏不稳定性，+1/2 对应速率随机游走。
- 读取曲线在 τ=1s 处的值得到白噪声密度，读取斜率0处的最低点得到零偏不稳定性系数。
# 使用 imu_utils 或 allan_variance_ros 的典型命令
rosrun allan_variance_ros allan_variance /path/to/bag imu_topic
rosrun allan_variance_ros analysis.py --data allan_variance.csv
# 输出示例
Gyro Noise Density: 0.0018 deg/s/√Hz
Gyro Bias Instability: 0.00012 deg/s
Accel Noise Density: 0.00025 m/s²/√Hz
Accel Bias Instability: 0.00004 m/s²四、误差参数与因子图配置对照
| 误差类型 | 数学符号 | 艾伦方差来源 | 因子图配置 |
|---|---|---|---|
| 白噪声 | σ_g, σ_a | τ=1s处曲线值 | 预积分噪声传播矩阵 Q |
| 零偏随机游走 | σ_bg, σ_ba | 斜率0处最低点 | 零偏节点过程噪声 |
| 尺度因子 | s_x, s_y, s_z | 不直接给出 | 标定后常量修正 |
| 非正交误差 | T 矩阵 | 不直接给出 | 标定后常量修正 |
五、典型误区与复盘
误区一：把零偏当常量。很多初学者在预积分中把零偏设为固定值，导致长时间运行后零偏漂移无法被吸收，轨迹出现缓慢弯曲。正确做法是把零偏作为随机游走节点，让因子图在线估计。
误区二：忽略尺度因子和非正交误差。在低成本MEMS中，尺度因子误差可达1%～3%，非正交误差可达0.5°～2°。如果不标定，GNSS/IMU融合在转弯和加减速时会出现系统性偏差。
误区三：艾伦方差数据质量差。采集时温度变化、桌面振动、电源纹波都会污染曲线。建议恒温静置，使用电池供电，并至少采集2小时以上。
误区四：噪声参数直接照搬数据手册。数据手册通常给出理想条件下的典型值，实际安装、温度、供电都会改变噪声水平。必须用自己设备的艾伦方差结果配置因子图。
总结来看，IMU误差建模的核心是把“可估计的状态”和“可标定的常量”分开处理：零偏进入因子图作为随机游走节点，白噪声进入预积分协方差传播，尺度因子和非正交误差通过离线标定固定。艾伦方差分析则是获取白噪声密度和零偏随机游走系数的标准手段，其输出参数直接决定因子图的信息矩阵配置。只有这四类误差都被正确建模，IMU预积分才能在融合定位中发挥应有的作用。