4. 多源数据融合方法
无人机多源定位的核心难题在于：GNSS、IMU、视觉、气压计、磁力计等传感器各有各的坐标系、采样率、噪声特性和失效模式。融合方法要回答三个问题——用什么状态量描述位姿、如何用观测修正预测、在传感器失效或切换时如何保持估计连续。本节按"滤波—优化"两条主线梳理六类主流方法，并给出适用场景判断依据。
一、核心原理：预测与校正的闭环
几乎所有融合方法都遵循同一套贝叶斯递推逻辑：用运动模型把上一时刻状态推到当前时刻（预测），再用当前观测对预测做加权修正（校正）。区别在于状态分布如何表示、线性化如何处理、历史信息保留多少。
- 卡尔曼滤波（KF）：假设状态与噪声均为高斯分布，系统线性。用均值向量和协方差矩阵完整描述不确定性，预测与更新均为闭式矩阵运算，计算量小、实时性最好。
- 扩展卡尔曼滤波（EKF）：系统非线性时，用一阶泰勒展开在工作点附近线性化。无人机姿态动力学、GNSS 伪距观测都是非线性的，EKF 是工程上最常用的折中方案。
- 无迹卡尔曼滤波（UKF）：不做线性化，而是选取一组 Sigma 点经非线性函数传播后再统计均值和协方差，能捕捉二阶以上非线性，强非线性场景下精度优于 EKF。
- 粒子滤波（PF）：用大量带权样本近似任意分布，可处理非高斯、多峰问题，如室内 GNSS 拒止下的多假设定位，但计算量随粒子数线性增长。
- 因子图优化（FGO）：把状态和观测建模为图节点与边，通过最小化所有历史观测的残差平方和求解全局最优，可回环、可重线性化，精度高但计算随历史增长。
- 互补滤波：利用不同传感器在频域上的互补特性，用高通保留 IMU 高频动态、低通保留 GNSS/磁力计低频基准，结构极简，常用于姿态估计。
二、方法流程与关键参数
以 EKF 为例，一个可运行的融合循环包含以下步骤。参数配置直接决定切换时的平滑程度。
# EKF 融合循环伪代码（15 维状态：p, v, q, ba, bg）
predict:
x = f(x, u, dt) # IMU 积分预测
F = jacobian(f, x) # 状态转移雅可比
P = F @ P @ F.T + Q # 协方差传播，Q 为过程噪声
update(gnss):
z_pred = h(x) # 预测观测
H = jacobian(h, x) # 观测雅可比
K = P @ H.T @ inv(H @ P @ H.T + R)
x = x + K @ (z - z_pred)
P = (I - K @ H) @ P
# 关键参数建议
Q_pos = 0.05 m^2/s^3 # 位置过程噪声
Q_att = 0.01 rad^2/s^3 # 姿态过程噪声
R_gnss = 1.5 m^2 # GNSS 观测噪声
R_vis = 0.3 m^2 # 视觉观测噪声
UKF 把上述雅可比替换为 Sigma 点传播，PF 把高斯分布替换为粒子集，FGO 则把每一步的残差加入图后统一求解。互补滤波最简单，姿态估计中常用形式为：angle = α·(angle + gyro·dt) + (1-α)·acc_angle，α 通常取 0.95～0.98。
三、适用场景对照
| 方法 | 分布假设 | 非线性处理 | 计算量 | 典型适用场景 |
|---|---|---|---|---|
| KF | 高斯 | 仅线性 | 极低 | 匀速直线、简单高度融合 |
| EKF | 高斯 | 一阶线性化 | 低 | GNSS/IMU 组合导航、姿态估计 |
| UKF | 高斯 | Sigma 点传播 | 中 | 强机动、大姿态角变化 |
| PF | 任意 | 样本近似 | 高 | 室内多假设、GNSS 拒止重定位 |
| FGO | 任意 | 迭代重线性化 | 高 | 视觉惯性里程计、回环校正 |
| 互补滤波 | 频域互补 | 不显式处理 | 极低 | 低成本飞控姿态解算 |
四、案例：GNSS 失效时的融合切换
某巡检无人机在楼宇间飞行，GNSS 由 12 颗星骤降至 3 颗，水平精度从 1.2 m 恶化到 8 m。此时若继续以固定 R 值融合，位置估计会被拖偏。工程做法是：
- 用新息协方差与理论协方差之比做卡方检验，判定 GNSS 观测异常；
- 将 R_gnss 动态放大 10～50 倍，降低 GNSS 权重，让 IMU 与视觉主导；
- 若持续失效超过 2 s，切换到视觉惯性里程计，并启动重定位线程；
- GNSS 恢复后不立即全权重接入，而是用 3～5 s 斜坡恢复，避免状态跳变。
五、常见误区与复盘
误区一：认为 UKF 一定优于 EKF。在弱非线性、高采样率场景，EKF 精度与 UKF 接近，但计算量更小。盲目上 UKF 可能挤占飞控算力。
误区二：忽略时间同步。IMU 200 Hz、GNSS 5 Hz、视觉 30 Hz，若不做时间对齐，融合残差中会混入运动误差，表现为"越融合越漂"。
误区三：FGO 窗口无限增长。不做滑窗或边缘化，计算量会随时间线性上升，最终无法实时。通常保留最近 5～10 s 关键帧。
误区四：互补滤波参数一成不变。α 应根据机动强度自适应调整，高动态时提高陀螺权重，悬停时提高加速度计权重。
复盘时建议记录三类数据：各传感器残差序列、协方差 P 的对角线变化、切换触发时刻与恢复时间。这三组数据能直接定位是模型不准、噪声参数失配，还是切换逻辑过于激进。