企业官网建设流程全解析

从滤波到优化：GraphGNSSLib源码解析，看因子图如何‘记住’历史数据提升GNSS鲁棒性

全球导航卫星系统（GNSS）在自动驾驶、无人机导航等领域扮演着关键角色，但城市峡谷中的多径效应和信号遮挡始终是难以回避的挑战。传统基于卡尔曼滤波（EKF）的方法虽在开阔环境中表现稳定，却无法有效利用历史测量数据来修正当前状态估计。这正是GraphGNSSLib选择因子图优化（FGO）作为核心架构的原因——通过构建时间窗口内的全局优化问题，系统能够同时考虑多个时刻的观测数据，显著提升复杂环境下的定位鲁棒性。

1. 因子图优化与传统滤波的本质差异

1.1 状态估计的两种范式

传统EKF方法遵循递推式更新机制，每个时刻仅基于当前观测和上一状态进行估计。这种"遗忘性"导致系统无法回溯修正历史误差，当单次观测出现异常时，错误会持续传播。而因子图优化采用批处理优化思路，将多个时刻的状态变量和观测约束共同建模为一个非线性最小二乘问题：

// 典型因子图优化问题构造 NonlinearFactorGraph graph; Values initialEstimate; for (int t = 0; t < window_size; ++t) { graph.add(PseudorangeFactor(...)); // 伪距因子 graph.add(DopplerFactor(...)); // 多普勒因子 initialEstimate.insert(t, state_guess); } LevenbergMarquardtOptimizer optimizer(graph, initialEstimate); Values result = optimizer.optimize();

1.2 时间相关性的建模能力

在城市峡谷场景中，GNSS信号质量往往呈现时间连续性——当接收机进入遮挡区域时，多个连续时刻的观测都会受到影响。因子图通过以下方式利用这种特性：

• 多历元状态节点互联：滑动窗口内的所有状态变量共同参与优化
• 自适应权重分配：根据卫星仰角和信噪比动态调整各观测的置信度
• 误差反向传播：当前时刻的可靠观测可以修正历史状态估计

下表对比两种方法的关键特性：

2. GraphGNSSLib的核心因子解析

2.1 伪距因子的实现精妙

伪距作为最基本的GNSS观测量，其因子实现需要考虑信号传播中的各类误差。GraphGNSSLib在PseudorangeFactor.h中定义了如下误差模型：

Vector PseudorangeFactor::evaluateError(const Pose3& pose, const Vector3& clock, OptionalMatrixType H1, OptionalMatrixType H2) const { Vector3 sat_to_rx = pose.translation() - sat_pos_; double range = sat_to_rx.norm(); double error = range + clock[0] - measured_pseudorange_; // 基于卫星仰角的协方差调整 double elevation = calculateElevation(pose.translation(), sat_pos_); double weight = 1.0 / (sigma_ * (1 + exp(-2*(elevation - 30)))); return (Vector(1) << error * weight).finished(); }

关键设计细节包括：

• 大气延迟补偿：使用RTKLIB模型校正电离层和对流层效应
• 多路径检测：通过信噪比(SNR)变化识别异常观测
• 自适应权重：卫星仰角越低，协方差权重越大（可靠性越低）

2.2 多普勒速度因子的动态约束

多普勒观测提供了接收机相对卫星的径向速度信息，在GraphGNSSLib中作为连接连续状态的关键纽带。其核心实现体现在：

Vector DopplerFactor::evaluateError(const Velocity& vel, const Vector1& clock_drift, OptionalMatrixType H1, OptionalMatrixType H2) const { Vector3 relative_vel = vel - sat_vel_; double radial_vel = relative_vel.dot(sat_to_rx_.normalized()); double predicted = radial_vel + clock_drift[0]; return (Vector(1) << (predicted - measured_doppler_) * weight_).finished(); }

注意：多普勒因子在信号中断期间尤为重要，其提供的速度约束可以防止位置估计发散

3. 协方差建模的艺术

3.1 基于卫星几何的质量评估

GraphGNSSLib创新性地将卫星几何分布信息融入协方差计算。在CovarianceEstimator.cpp中可见：

def compute_covariance(sat_positions, receiver_pos): H = [] for sat in sat_positions: line_of_sight = (sat - receiver_pos).normalized() H.append([*line_of_sight, 1]) # 位置+钟差参数 H_matrix = np.array(H) Q = np.diag([1/(snr**2) for snr in snr_list]) # SNR加权 cov = np.linalg.inv(H_matrix.T @ Q @ H_matrix) return cov[:3,:3] # 仅提取位置协方差

这种设计使得系统能够：

1. 自动降低低仰角卫星的贡献度
2. 在卫星几何分布不佳时增大位置不确定度
3. 动态调整各观测量的相对权重

3.2 时间相关噪声的处理

针对连续多历元的观测相关性，代码采用移动平均策略平滑协方差估计：

void updateNoiseModel(const std::vector<Measurement>& window_meas) { for (int i = 0; i < window_size-1; ++i) { double temporal_corr = calculateCorrelation(window_meas[i], window_meas[i+1]); covariance_matrix_ *= (1.0 - temporal_corr); } }

4. 实战：城市峡谷场景下的性能对比

4.1 测试环境配置

使用香港中环数据集进行评测，参数设置如下：

4.2 典型场景分析

案例1：短暂信号遮挡当车辆通过高架桥时，EKF轨迹出现明显跳跃（最大误差8.3米），而FGO通过前后历元的约束将误差控制在2.1米内。这是因为：

1. 遮挡前的健康观测提供了强先验
2. 多普勒因子维持了合理的速度约束
3. 优化过程自动降低了遮挡期间观测的权重

案例2：多路径干扰区域在玻璃幕墙密集区，FGO表现出独特的优势：

• 通过连续多个历元的残差分析识别出受污染卫星
• 动态调整协方差矩阵降低异常观测影响
• 利用历史可靠数据补偿当前误差

提示：实际部署时可结合3D城市模型预判多路径风险区域，进一步优化权重策略

5. 扩展与定制指南

5.1 多传感器融合接口

GraphGNSSLib设计了简洁的接口用于接入其他传感器数据。以激光雷达里程计为例：

class LidarOdometryFactor : public NoiseModelFactor1<Pose3> { public: LidarOdometryFactor(Key key, const Pose3& odom, const SharedNoiseModel& model) : NoiseModelFactor1<Pose3>(model, key), odom_(odom) {} Vector evaluateError(const Pose3& pose, OptionalMatrixType H) const override { if (H) *H = eye(6); return pose.localCoordinates(odom_); } private: Pose3 odom_; };

5.2 参数调优建议

根据实际场景特点，可调整以下关键参数：

• 窗口大小：市区建议5-15历元，开阔环境可减小
• 收敛阈值：严格阈值(1e-6)适合高精度场景
• 鲁棒核函数：CauchyLoss对异常值更鲁棒
• 线程配置：启用并行化加速大窗口优化

在开源社区的实际应用中，开发者们发现将载波相位权重设置为伪距的50-100倍，并在信号遮挡时临时放宽收敛条件，能获得最佳平衡。