3. 定位算法入门

自动驾驶定位系统要回答的核心问题只有一个：车辆此刻在哪里、朝向如何、速度多大。传感器给出的原始观测总是带有噪声、延迟和局部失效，因此必须依靠状态估计算法把多源信息融合成连续、平滑、可用的位姿轨迹。卡尔曼滤波、粒子滤波和图优化是三类最基础也最常用的定位算法，它们分别代表了递推估计、非参数贝叶斯估计和批量优化三种思路。理解它们的适用边界，是后续量产验证工作的前提。

一、核心原理：三种估计思路的差异

卡尔曼滤波（KF）假设系统状态服从高斯分布，通过“预测—更新”两步递推完成估计。预测步用运动模型把上一时刻状态外推到当前时刻，更新步用观测模型修正预测值。对于非线性系统，扩展卡尔曼滤波（EKF）在工作点附近做一阶线性化，无迹卡尔曼滤波（UKF）则用sigma点传播均值和协方差。KF类算法计算量小、实时性好，是组合导航和IMU/轮速融合的主力。

粒子滤波（PF）用一组带权重的随机样本近似后验分布，不要求高斯假设，能表达多峰、强非线性分布。它天然适合全局定位、多假设跟踪和激光雷达地图匹配等场景，但计算量随粒子数增长，且存在粒子退化问题，需要重采样策略配合。

图优化把定位问题建模为因子图：位姿和路标是节点，观测和运动约束是边，通过最小化所有边的残差平方和求解最优状态。它属于批量优化方法，能利用历史全部信息，精度通常高于滤波，但需要求解稀疏线性系统，适合离线建图、回环校正和SLAM后端。

二、方法流程：从预测到修正的闭环

以EKF组合导航为例，一个完整的定位周期包含以下步骤：

1. 状态定义：确定状态向量，例如位置、速度、姿态、IMU零偏，共15维。
2. 时间更新：用IMU角速度和比力积分，得到预测状态和协方差矩阵。
3. 观测更新：当GNSS或轮速观测到达时，计算卡尔曼增益，修正状态和协方差。
4. 异常处理：通过新息卡方检验判断观测是否可信，剔除多路径或跳变观测。
5. 输出与监控：输出位姿及其不确定度，监控协方差是否发散。

粒子滤波的流程则是：初始化粒子群、按运动模型传播、按观测模型计算权重、重采样、输出加权均值。图优化的流程是：构建因子图、选择鲁棒核函数、用高斯牛顿或LM迭代求解、边缘化旧状态。

三、案例场景：城市峡谷中的定位融合

某量产项目在城区主干道测试，车辆以40km/h行驶，两侧高楼导致GNSS频繁失锁。系统采用EKF作为主滤波器，融合IMU、轮速和GNSS；当GNSS连续失锁超过2秒时，启动激光雷达粒子滤波进行地图匹配，提供绝对位置观测。实测表明，纯EKF在失锁30秒后横向误差可达8米，而加入粒子滤波校正后误差控制在0.5米以内。

# 典型EKF观测更新伪代码
K = P_pred @ H.T @ inv(H @ P_pred @ H.T + R)
x_est = x_pred + K @ (z - h(x_pred))
P_est = (I - K @ H) @ P_pred

# 新息卡方检验
innovation = z - h(x_pred)
S = H @ P_pred @ H.T + R
if innovation.T @ inv(S) @ innovation > threshold:
    reject_observation()

四、误区与复盘：算法选型的常见陷阱