1. 从GPS定位看两种算法的本质差异第一次接触GPS定位时我拿着开发板在户外测试发现一个有趣现象静止时定位点总在小范围跳动而移动时轨迹却像喝醉似的左右摇摆。这背后其实是最小二乘法和卡尔曼滤波两种算法哲学的对决。就像用两种不同的方式观察世界——前者像拍快照后者像看连续剧。在伪距单点定位中接收机通过测量与至少4颗卫星的距离含误差来计算自身位置。最小二乘法的处理逻辑特别直白把当前时刻所有观测数据扔进方程组求出一个让误差平方和最小的解。这就像用手机拍张全景照片——不管前面拍得多糟这一张永远从零开始。实测发现在静态场景下这种活在当下的特性让它前几次迭代就能快速逼近真实位置我用Ublox模块测试时冷启动后3秒内就能收敛到10米精度。但问题也随之而来。有次我在高架桥下测试突然出现的多路径误差导致定位点偏移了20多米。最小二乘就像个固执的画家每一笔都只参考当前看到的景象哪怕上一秒画的是蓝天下一秒也可能因为乌云突然改画暴雨。这种记忆缺失导致其抗干扰能力较弱我在城市峡谷环境测得的数据显示其水平定位误差经常达到15米以上。2. 最小二乘法的快与痛2.1 闪电般的初始收敛在树莓派上跑定位算法时最小二乘的表现令人惊艳。它的核心优势在于计算简单——本质上就是个矩阵运算# 最小二乘定位核心代码示例 import numpy as np def least_square(A, b): return np.linalg.inv(A.T A) A.T b这种简洁性带来三个实战优势冷启动友好不需要任何先验位置信息我在车库测试时即使初始坐标设为零点5次迭代内就能收敛计算开销低在STM32F4芯片上单次解算仅需2ms并行化容易适合多星座系统GPS北斗GLONASS同时处理但它的代价也很明显。去年做无人机项目时我发现当卫星几何构型较差如PDOP3时最小二乘解的精度会断崖式下降。有组实测数据很能说明问题场景水平误差(m)收敛时间(s)开阔地3.22.1城市峡谷18.73.5高架桥下25.44.82.2 独立观测的致命伤最小二乘把每个观测历元视为独立事件这就像用独立照片拼视频——缺少帧间关联必然导致跳变。我遇到过最极端的情况是静态观测时相邻两秒的定位点竟相差12米其根本局限在于误差传递失控伪距测量中的电离层延迟、钟差等误差被全盘接收动态响应迟钝对于时速60km的车辆这种健忘症会导致轨迹像折线而非曲线精度天花板低即使用RAIM等技术优化平面精度也很难突破3米有个很形象的类比最小二乘就像用体温计量体温——每次测量都是独立的无法判断体温是在上升还是下降。3. 卡尔曼滤波的时空观3.1 状态空间的魔法第一次实现卡尔曼滤波时我被它的预测-更新机制震撼了。它用状态方程建模运动规律x_k F·x_{k-1} B·u_k w_k z_k H·x_k v_k这就像给定位系统装了物理引擎。在车载测试中即使GPS信号短暂丢失基于速度模型的预测仍能维持合理轨迹。有组对比数据很有趣断流时长(s)最小二乘漂移(m)卡尔曼漂移(m)18.22.1325.76.35失控14.8但卡尔曼滤波对初始值极其敏感。记得有次将初始位置误设为北京实际在上海结果滤波花了2分钟才收敛。其状态协方差矩阵P就像个信任天平初始P太大过度依赖观测早期波动剧烈初始P太小模型僵化难以修正错误3.2 动态追踪的双刃剑在无人机追踪项目中卡尔曼滤波展现了惊人潜力。通过设计合理的Q过程噪声和R观测噪声矩阵Q np.diag([0.1,0.1,0.5,0.5]) # 位置/速度过程噪声 R np.diag([5.0,5.0]) # 观测噪声我们实现了0.8m/s速度变化下的平滑追踪。但其代价是计算复杂度高每次迭代需维护状态协方差矩阵在ARM Cortex-M7上需8ms模型依赖强匀速模型在车辆转弯时会产生滞后发散风险大异常观测可能导致滤波崩溃有个实用技巧用新息Innovation检测异常值。当 (z-Hx)^T·S^{-1}·(z-Hx) χ²阈值时应触发保护机制。4. 工程实践的黄金法则4.1 场景化选型指南经过多个项目实战我总结出这样的决策树冷启动阶段先用最小二乘获取初始解静态/低速场景运动模型不可靠时适当增大过程噪声Q高速动态场景采用自适应卡尔曼滤波根据动力学调整Q信号遮挡环境结合惯性测量单元(IMU)构建组合导航特别提醒卡尔曼滤波的参数调试需要真实场景数据。我通常这样操作先在开阔地采集干净数据逐步添加城市/峡谷等复杂环境数据用MATLAB/Octave进行蒙特卡洛仿真4.2 混合策略实战案例在最近的AGV项目中我们采用了一种混合架构前端用最小二乘做快速解算100Hz后端用卡尔曼滤波做平滑10Hz当新息超过阈值时触发最小二乘重新初始化这种架构兼顾了响应速度和平稳性实测显示指标纯最小二乘纯卡尔曼混合方案冷启动时间(s)2.18.72.3动态误差(m)6.21.82.1计算负载(%)123522最后分享一个血泪教训永远要监控滤波器的归一化新息平方NIS。有次现场故障就是因为没发现IMU安装偏差导致模型逐渐失配NIS值持续偏高却未触发告警。现在我们的系统只要NIS连续5次超限就自动切换至最小二乘模式并记录诊断数据。
