3. GPS定位解算：从伪距到坐标
GPS定位解算，说白了就是回答一个问题：我到底在哪儿？
卫星在天上飞，发来一堆信号。你的接收机收到这些信号，算出距离，再解方程。嗯，听起来简单。但实际做起来，坑不少。
我个人习惯把定位解算拆成三步：
1. 测距——怎么知道卫星离我多远？
2. 解算——有了距离，怎么算坐标？
3. 修正——算出来不准，怎么补救？
这一章，我们就按这个顺序来聊。
3.1 伪距测量原理
先问个问题：你怎么测量你和一颗卫星之间的距离？
卫星发射信号，信号以光速传播。接收机记录信号到达的时间。如果知道信号是什么时候发出的，那距离就是：
距离 = 光速 × (接收时间 - 发射时间)但问题来了——接收机的时间不准。
卫星上有原子钟，精度极高。但你的手机、车载GPS接收机，用的是石英钟，一天能差好几秒。你想想看，光速是3×10⁸ m/s，差1微秒就是300米。所以直接算出来的距离，不是真实距离，而是伪距（Pseudorange）。
伪距 = 真实距离 + 接收机钟差 × 光速 + 各种误差
伪距的“伪”，就伪在它包含了接收机钟差。我们不知道钟差是多少，所以把它当成一个未知数，一起解算。
伪距观测方程长这样：
ρᵢ = √[(xᵢ - x)² + (yᵢ - y)² + (zᵢ - z)²] + c·δt + εᵢ其中：
ρᵢ —— 第i颗卫星的伪距观测值
(xᵢ, yᵢ, zᵢ) —— 卫星坐标（从星历中获取）
(x, y, z) —— 接收机坐标（待求）
c —— 光速
δt —— 接收机钟差（待求）
εᵢ —— 其他误差（电离层、对流层、多路径等）
这里有4个未知数：x, y, z, δt。所以理论上，至少需要4颗卫星才能解算。
我的经验： 在城市峡谷里，经常只能看到3颗卫星。这时候定位精度会急剧下降。我曾经在深圳华强北测试，高楼林立，卫星信号被遮挡，定位误差能到50米以上。嗯，这时候就得靠其他传感器（比如IMU）来辅助了。
3.2 最小二乘法定位解算
有了4颗以上的卫星，我们就有了一组非线性方程。怎么解？
最经典的方法就是最小二乘法（Least Squares, LS）。
思路是这样的：
- 线性化：把非线性方程在初始估计点附近泰勒展开，忽略高阶项，变成线性方程。
- 迭代求解：用线性最小二乘法求出修正量，更新估计点，再重复，直到收敛。
具体步骤：
1. 设定初始估计位置 (x₀, y₀, z₀) 和钟差 δt₀
通常取 (0, 0, 0) 或上一次定位结果
2. 计算每个卫星的预测伪距：
ρ̂ᵢ = √[(xᵢ - x₀)² + (yᵢ - y₀)² + (zᵢ - z₀)²] + c·δt₀
3. 计算观测残差：
Δρᵢ = ρᵢ - ρ̂ᵢ
4. 构建设计矩阵 H（每颗卫星一行，共4列）：
Hᵢ = [ -(xᵢ - x₀)/rᵢ, -(yᵢ - y₀)/rᵢ, -(zᵢ - z₀)/rᵢ, 1 ]
其中 rᵢ = √[(xᵢ - x₀)² + (yᵢ - y₀)² + (zᵢ - z₀)²]
5. 最小二乘解：
ΔX = (Hᵀ·H)⁻¹ · Hᵀ · Δρ
6. 更新状态：
x₀ += ΔX[0], y₀ += ΔX[1], z₀ += ΔX[2], δt₀ += ΔX[3]
7. 重复步骤2-6，直到 ΔX 足够小（比如 < 1e-6）注意： 设计矩阵H的第四列是1，对应钟差。这个1很重要，它表示钟差对所有卫星的伪距影响是一样的。我曾经见过新手把这一列写成0，结果迭代死活不收敛。
最小二乘法的核心，就是让所有卫星的残差平方和最小。它不挑卫星，一视同仁。但如果某颗卫星的观测质量特别差（比如多路径严重），它会把整体结果拉偏。
我建议在实际项目中，先做粗差检测，把明显异常的卫星剔除掉，再用最小二乘法。否则，一颗坏卫星能毁掉整个解。
3.3 单点定位 vs 差分定位
最小二乘法算出来的位置，精度够吗？
看场景。
单点定位（SPP, Standard Point Positioning）：只用一台接收机，直接解算。精度一般在5-15米（民用GPS，无SA干扰）。
为什么这么差？因为伪距里的误差太多了：
- 卫星轨道误差：几米
- 卫星钟差：几米
- 电离层延迟：几米到十几米
- 对流层延迟：几米
- 多路径效应：几米到几十米
- 接收机噪声：亚米级
这些误差加起来，十几米很正常。
差分定位（DGPS, Differential GPS）：用两台接收机，一台在已知坐标的基准站，一台在流动站。基准站算出伪距误差，发给流动站，流动站用这个误差修正自己的观测值。
为什么差分有效？因为空间相关性。基准站和流动站距离不远（几十公里内），它们看到的卫星相同，电离层、对流层、轨道误差也高度相关。差分之后，这些公共误差被大幅抵消。
| 对比项 | 单点定位 | 差分定位 |
|---|---|---|
| 设备数量 | 1台接收机 | 2台（基准站+流动站） |
| 精度 | 5-15米 | 亚米级（0.5-3米） |
| 实时性 | 实时 | 需数据链路传输修正量 |
| 覆盖范围 | 全球 | 受限于基准站覆盖（~100km） |
| 主要误差源 | 电离层、轨道、钟差 | 多路径、接收机噪声 |
| 典型应用 | 手机导航、车载导航 | 农业机械、测量测绘 |
一句话总结： 单点定位告诉你“我在哪条街上”，差分定位告诉你“我在哪个车道”。
我在做农机自动驾驶项目时，用的就是差分定位。拖拉机在田里跑，误差超过10厘米就会压到庄稼。单点定位根本不行，必须上差分。基准站架在田头，流动站装在拖拉机上，精度能到2-3厘米。嗯，这才是能用的水平。
3.4 本章知识体系
下面这张图，把本章的核心逻辑串起来了：
3.5 避坑指南
最后，分享几个我在项目中踩过的坑：
- 卫星几何分布：卫星都挤在天上一边，定位精度会很差。用DOP（精度衰减因子）来评估，DOP值越小越好。我一般要求GDOP < 4。
- 粗差剔除：最小二乘法对粗差敏感。我习惯先用RAIM（接收机自主完好性监测）检测，剔除异常卫星后再解算。
- 迭代初值：如果初始位置离真实位置太远（比如差几百公里），迭代可能发散。我建议用上一次定位结果作为初值，或者用重心法估算一个大概位置。
- 差分数据链路：差分修正量需要实时传输。如果链路断了，流动站会瞬间退化成单点定位。我在项目里加了超时检测，一旦差分数据中断超过10秒，自动切换回单点模式，并给出告警。
嗯，这一章的内容就到这里。伪距测量是基础，最小二乘法是工具，单点和差分是两种策略。理解了这些，GPS定位解算的核心你就掌握了。