GPS从入门到放弃（十） --- 定位方程解算和定位精度

GPS从入门到放弃（十） — 定位方程解算和定位精度

上一篇伪距与载波相位中我们介绍了伪距的计算方法，也得到了包含 (x,y,z,δt)(x,\ y,\ z,\ \delta_t)(x, y, z, δt) 四个未知数的GPS定位基本方程：
(x−xs)2+(y−ys)2+(z−zs)2+c⋅δt=ρ+c⋅δt,s−cI−cT−cϵ\sqrt{(x-x_{s})^2 + (y-y_{s})^2 + (z-z_{s})^2} + c\cdot\delta_t = \rho + c\cdot\delta_{t,s} - cI - cT -c\epsilon (x−xs)2+(y−ys)2+(z−zs)2+c⋅δt=ρ+c⋅δt,s−cI−cT−cϵ
那么根据这个方程我们怎么来定位呢？

这个方程中的 III 和 TTT 分别是大气电离层导致的延时和大气对流层导致的延时，这些延时的计算方法放到后面再讲，目前我们先把它当作已知量。δt,s\delta_{t,s}δt,s 为卫星钟差，在导航电文中有参数修正，以后再讲。于是这个方程中只涉及到四个未知数和一个误差。我们先考虑简单的情况，即暂时不管误差 ϵ\epsilonϵ ，在分析定位精度的时候再来考虑它。

根据我们第一篇GPS基础原理讲过GPS的基本原理，只需已知四颗卫星的测量值，即可组成一个四元方程组，然后解出来这四个未知数。要注意的是这个方程组是一个非线性方程组，因此在实际解算过程中，常用牛顿迭代法来进行。

牛顿迭代法

牛顿迭代法是一个常用的解非线性方程组的方法，它将非线性方程组在一个估计解的附近进行线性化，然后求解线性化后的方程组，接着再更新解的估计值。如此反复迭代，直到解的精度满足要求为止。

根据牛顿迭代法，将四元方程组在第k次迭代的估计解 [xkykzkδt,k]T[x_k \ \ y_k \ \ z_k\ \ \delta_{t,k}]^T[xk yk zk δt,k]T 处线性化后方程组为：
G[ΔxΔyΔzc⋅Δδt]=b\boldsymbol{G} \left[ \begin{array}{c} \Delta x\\ \Delta y\\ \Delta z\\ c\cdot\Delta \delta_{t} \end{array} \right] = \boldsymbol{b} G⎣⎢⎢⎡ΔxΔyΔzc⋅Δδt⎦⎥⎥⎤=b
其中
G=[xk−xs,1(xk−xs,1)2+(yk−ys,1)2+(zk−zs,1)2yk−ys,1(xk−xs,1)2+(yk−ys,1)2+(zk−zs,1)2zk−zs,1(xk−xs,1)2+(yk−ys,1)2+(zk−zs,1)21xk−xs,2(xk−xs,2)2+(yk−ys,2)2+(zk−zs,2)2yk−ys,2(xk−xs,2)2+(yk−ys,2)2+(zk−zs,2)2zk−zs,2(xk−xs,2)2+(yk−ys,2)2+(zk−zs,2)21xk−xs,3(xk−xs,3)2+(yk−ys,3)2+(zk−zs,3)2yk−ys,3(xk−xs,3)2+(yk−ys,3)2+(zk−zs,3)2zk−zs,3(xk−xs,3)2+(yk−ys,3)2+(zk−zs,3)21xk−xs,4(xk−xs,4)2+(yk−ys,4)2+(zk−zs,4)2yk−ys,4(xk−xs,4)2+(yk−ys,4)2+(zk−zs,4)2zk−zs,4(xk−xs,4)2+(yk−ys,4)2+(zk−zs,4)21]\boldsymbol{G} = \left[ \begin{array}{cccc} \frac{x_k-x_{s,1}}{\sqrt{(x_k-x_{s,1})^2 + (y_k-y_{s,1})^2 + (z_k-z_{s,1})^2}} & \frac{y_k-y_{s,1}}{\sqrt{(x_k-x_{s,1})^2 + (y_k-y_{s,1})^2 + (z_k-z_{s,1})^2}} & \frac{z_k-z_{s,1}}{\sqrt{(x_k-x_{s,1})^2 + (y_k-y_{s,1})^2 + (z_k-z_{s,1})^2}} & 1\\ \frac{x_k-x_{s,2}}{\sqrt{(x_k-x_{s,2})^2 + (y_k-y_{s,2})^2 + (z_k-z_{s,2})^2}} & \frac{y_k-y_{s,2}}{\sqrt{(x_k-x_{s,2})^2 + (y_k-y_{s,2})^2 + (z_k-z_{s,2})^2}} & \frac{z_k-z_{s,2}}{\sqrt{(x_k-x_{s,2})^2 + (y_k-y_{s,2})^2 + (z_k-z_{s,2})^2}} & 1\\ \frac{x_k-x_{s,3}}{\sqrt{(x_k-x_{s,3})^2 + (y_k-y_{s,3})^2 + (z_k-z_{s,3})^2}} & \frac{y_k-y_{s,3}}{\sqrt{(x_k-x_{s,3})^2 + (y_k-y_{s,3})^2 + (z_k-z_{s,3})^2}} & \frac{z_k-z_{s,3}}{\sqrt{(x_k-x_{s,3})^2 + (y_k-y_{s,3})^2 + (z_k-z_{s,3})^2}} & 1\\ \frac{x_k-x_{s,4}}{\sqrt{(x_k-x_{s,4})^2 + (y_k-y_{s,4})^2 + (z_k-z_{s,4})^2}} & \frac{y_k-y_{s,4}}{\sqrt{(x_k-x_{s,4})^2 + (y_k-y_{s,4})^2 + (z_k-z_{s,4})^2}} & \frac{z_k-z_{s,4}}{\sqrt{(x_k-x_{s,4})^2 + (y_k-y_{s,4})^2 + (z_k-z_{s,4})^2}} & 1 \end{array} \right] G=⎣⎢⎢⎢⎢⎢⎡(xk−xs,1)2+(yk−ys,1)2+(zk−zs,1)2xk−xs,1(xk−xs,2)2+(yk−ys,2)2+(zk−zs,2)2xk−xs,2(xk−xs,3)2+(yk−ys,3)2+(zk−zs,3)2xk−xs,3(xk−xs,4)2+(yk−ys,4)2+(zk−zs,4)2xk−xs,4(xk−xs,1)2+(yk−ys,1)2+(zk−zs,1)2yk−ys,1(xk−xs,2)2+(yk−ys,2)2+(zk−zs,2)2yk−ys,2(xk−xs,3)2+(yk−ys,3)2+(zk−zs,3)2yk−ys,3(xk−xs,4)2+(yk−ys,4)2+(zk−zs,4)2yk−ys,4(xk−xs,1)2+(yk−ys,1)2+(zk−zs,1)2zk−zs,1(xk−xs,2)2+(yk−ys,2)2+(zk−zs,2)2zk−zs,2(xk−xs,3)2+(yk−ys,3)2+(zk−zs,3)2zk−zs,3(xk−xs,4)2+(yk−ys,4)2+(zk−zs,4)2zk−zs,41111⎦⎥⎥⎥⎥⎥⎤
b=[ρ1+c⋅δt,s,1−cI1−cT1−(xk−xs,1)2+(yk−ys,1)2+(zk−zs,1)2−c⋅δt,kρ2+c⋅δt,s,2−cI2−cT2−(xk−xs,2)2+(yk−ys,2)2+(zk−zs,2)2−c⋅δt,kρ3+c⋅δt,s,3−cI3−cT3−(xk−xs,3)2+(yk−ys,3)2+(zk−zs,3)2−c⋅δt,kρ4+c⋅δt,s,4−cI4−cT4−(xk−xs,4)2+(yk−ys,4)2+(zk−zs,4)2−c⋅δt,k]\boldsymbol{b} = \left[ \begin{array}{cccc} \rho_1 + c\cdot\delta_{t,s,1} - cI_1 - cT_1 -\sqrt{(x_k-x_{s,1})^2 + (y_k-y_{s,1})^2 + (z_k-z_{s,1})^2} - c\cdot\delta_{t,k} \\ \rho_2 + c\cdot\delta_{t,s,2} - cI_2 - cT_2 -\sqrt{(x_k-x_{s,2})^2 + (y_k-y_{s,2})^2 + (z_k-z_{s,2})^2} - c\cdot\delta_{t,k} \\ \rho_3 + c\cdot\delta_{t,s,3} - cI_3 - cT_3 -\sqrt{(x_k-x_{s,3})^2 + (y_k-y_{s,3})^2 + (z_k-z_{s,3})^2} - c\cdot\delta_{t,k} \\ \rho_4 + c\cdot\delta_{t,s,4} - cI_4 - cT_4 -\sqrt{(x_k-x_{s,4})^2 + (y_k-y_{s,4})^2 + (z_k-z_{s,4})^2} - c\cdot\delta_{t,k} \end{array} \right] b=⎣⎢⎢⎡ρ1+c⋅δt,s,1−cI1−cT1−(xk−xs,1)2+(yk−ys,1)2+(zk−zs,1)2−c⋅δt,kρ2+c⋅δt,s,2−cI2−cT2−(xk−xs,2)2+(yk−ys,2)2+(zk−zs,2)2−c⋅δt,kρ3+c⋅δt,s,3−cI3−cT3−(xk−xs,3)2+(yk−ys,3)2+(zk−zs,3)2−c⋅δt,kρ4+c⋅δt,s,4−cI4−cT4−(xk−xs,4)2+(yk−ys,4)2+(zk−zs,4)2−c⋅δt,k⎦⎥⎥⎤
我们将 G\boldsymbol{G}G 称为雅可比矩阵。
设第k次迭代时接收机到卫星 sss 的单位观测矢量为 es,k=[es,k,x,es,k,y,es,k,z]T\boldsymbol{e}_{s,k}=[e_{s,k,x},\ e_{s,k,y},\ e_{s,k,z}]^Tes,k=[es,k,x, es,k,y, es,k,z]T，则 G\boldsymbol{G}G 可以写为：
G=[−e1,k,x−e1,k,y−e1,k,z1−e2,k,x−e2,k,y−e2,k,z1−e3,k,x−e3,k,y−e3,k,z1−e4,k,x−e4,k,y−e4,k,z1]=[−e1,k1−e2,k1−e3,k1−e4,k1]\boldsymbol{G} = \left[ \begin{array}{cccc} -e_{1,k,x} & -e_{1,k,y} & -e_{1,k,z} & 1\\ -e_{2,k,x} & -e_{2,k,y} & -e_{2,k,z} & 1\\ -e_{3,k,x} & -e_{3,k,y} & -e_{3,k,z} & 1\\ -e_{4,k,x} & -e_{4,k,y} & -e_{4,k,z} & 1 \end{array} \right] = \left[ \begin{array}{cc} -\boldsymbol{e}_{1,k} & 1\\ -\boldsymbol{e}_{2,k} & 1\\ -\boldsymbol{e}_{3,k} & 1\\ -\boldsymbol{e}_{4,k} & 1 \end{array} \right] G=⎣⎢⎢⎡−e1,k,x−e2,k,x−e3,k,x−e4,k,x−e1,k,y−e2,k,y−e3,k,y−e4,k,y−e1,k,z−e2,k,z−e3,k,z−e4,k,z1111⎦⎥⎥⎤=⎣⎢⎢⎡−e1,k−e2,k−e3,k−e4,k1111⎦⎥⎥⎤
观察 G\boldsymbol{G}G 可以发现，G\boldsymbol{G}G 只与卫星和接收机的几何位置有关，所以也称 G\boldsymbol{G}G 为几何矩阵。

而一般把 b\boldsymbol{b}b 称为伪距残差。它是观测到的伪距与第k次迭代时估计出的伪距的差值。

得到线性化的方程组之后，我们就可以用最小二乘法将方程组解出来，得到
[ΔxΔyΔzc⋅Δδt]=(GTG)−1GTb\left[ \begin{array}{c} \Delta x\\ \Delta y\\ \Delta z\\ c\cdot\Delta \delta_{t} \end{array} \right] = (\boldsymbol{G}^T\boldsymbol{G})^{-1}\boldsymbol{G}^T\boldsymbol{b} ⎣⎢⎢⎡ΔxΔyΔzc⋅Δδt⎦⎥⎥⎤=(GTG)−1GTb
然后进一步得到迭代下一步的估计值
[xk+1yk+1zk+1δt,k+1]=[xk+Δxyk+Δyzk+Δzδt,k+Δδt]\left[ \begin{array}{c} x_{k+1}\\ y_{k+1}\\ z_{k+1}\\ \delta_{t,k+1} \end{array} \right] = \left[ \begin{array}{c} x_{k} + \Delta x\\ y_{k} + \Delta y\\ z_{k} + \Delta z\\ \delta_{t,k} + \Delta \delta_{t} \end{array} \right] ⎣⎢⎢⎡xk+1yk+1zk+1δt,k+1⎦⎥⎥⎤=⎣⎢⎢⎡xk+Δxyk+Δyzk+Δzδt,k+Δδt⎦⎥⎥⎤
如此反复迭代，直到 [xkykzkδt,k]T[x_k \ \ y_k \ \ z_k\ \ \delta_{t,k}]^T[xk yk zk δt,k]T 满足精度要求，牛顿迭代法即可中止。

在使用牛顿迭代法解算位置的过程中，需要注意几点：

是否收敛。解的估计值有可能在一个值附近来回振荡，这是无法得到更高精度的解。
是否收敛到地球附近位置。解有可能收敛到远离地球的一端，这时需要重新给初始值，重新进行迭代解算。
严格来讲，每次迭代位置更新后，大气延时等误差需要重新估算，为了减小计算量，在连续定位时可以认为此误差在迭代中保持不变。
若可观测卫星多于4颗，可以对雅可比矩阵 G\boldsymbol{G}G 进行扩展，依然可以用牛顿迭代和最小二乘法来求解。
若可观测卫星少于4颗，可以利用各种假设来增加辅助方程，以解出需要的未知数。如限定高度变化量、限定运动方向、限定接收机钟差变化量等，当然此处需要注意限定的有效期。

定位精度

下面我们把误差也考虑进去，假定测量误差和定位误差都很小，于是线性化后方程组为：
G[Δx+ϵxΔy+ϵyΔz+ϵzc⋅(Δδt+ϵδt)]=b+ϵρ\boldsymbol{G} \left[ \begin{array}{c} \Delta x + \epsilon_x\\ \Delta y + \epsilon_y\\ \Delta z + \epsilon_z\\ c\cdot(\Delta \delta_{t} + \epsilon_{\delta_{t}}) \end{array} \right] = \boldsymbol{b + \epsilon_\rho} G⎣⎢⎢⎡Δx+ϵxΔy+ϵyΔz+ϵzc⋅(Δδt+ϵδt)⎦⎥⎥⎤=b+ϵρ
其中
ϵρ=[ϵρ,1ϵρ,2ϵρ,3ϵρ,4]\boldsymbol{\epsilon_\rho} = \left[ \begin{array}{c} \epsilon_{\rho,1}\\ \epsilon_{\rho,2}\\ \epsilon_{\rho,3}\\ \epsilon_{\rho,4} \end{array} \right] ϵρ=⎣⎢⎢⎡ϵρ,1ϵρ,2ϵρ,3ϵρ,4⎦⎥⎥⎤
为卫星的测量误差向量，ϵx,ϵy,ϵz\epsilon_x, \epsilon_y, \epsilon_zϵx,ϵy,ϵz 和 ϵδt\epsilon_{\delta_t}ϵδt 分别表示由误差向量 ϵρ\boldsymbol{\epsilon_\rho}ϵρ 引起的定位和定时误差。

解这个方程可以得到
[ϵxϵyϵzϵδt]=(GTG)−1GTϵρ\left[ \begin{array}{c} \epsilon_x\\ \epsilon_y\\ \epsilon_z\\ \epsilon_{\delta_{t}} \end{array} \right] = (\boldsymbol{G}^T\boldsymbol{G})^{-1}\boldsymbol{G}^T\boldsymbol{\epsilon_\rho} ⎣⎢⎢⎡ϵxϵyϵzϵδt⎦⎥⎥⎤=(GTG)−1GTϵρ
假设各个卫星的测量误差都为正态分布，其均值 E[ϵρ]=0E[\epsilon_\rho] = 0E[ϵρ]=0，方差 V[ϵρ]=σURE2V[\epsilon_\rho] = \sigma_{URE}^2V[ϵρ]=σURE2，假设各个卫星的测量误差互不相关，则可以推导出定位误差协方差阵为：
Cov([ϵxϵyϵzϵδt])=(GTG)−1σURE2=HσURE2Cov\left(\left[ \begin{array}{c} \epsilon_x\\ \epsilon_y\\ \epsilon_z\\ \epsilon_{\delta_{t}} \end{array} \right]\right) = (\boldsymbol{G}^T\boldsymbol{G})^{-1}\sigma_{URE}^2 = \boldsymbol{H}\sigma_{URE}^2 Cov⎝⎜⎜⎛⎣⎢⎢⎡ϵxϵyϵzϵδt⎦⎥⎥⎤⎠⎟⎟⎞=(GTG)−1σURE2=HσURE2
其中
H=(GTG)−1\boldsymbol{H} = (\boldsymbol{G}^T\boldsymbol{G})^{-1} H=(GTG)−1
为权系数阵，是一个对称阵。

由定位误差协方差阵可以看出，GPS定位误差的方差是测量误差的方差被权系数阵放大的结果，而权系数阵只与卫星的几何分布有关，故GPS的定位误差取决于测量误差和卫星几何分布两个因素。

精度因子

有了权系数阵，我们就可以计算精度因子了。精度因子用于表示各个方向和时钟的误差放大倍数。假设在站心坐标系（坐标系可参见前文GPS坐标系）下表示的权系数阵为：
H=[h11h22h33h44]\boldsymbol{H} = \left[ \begin{array}{cccc} h_{11} & & & \\ & h_{22} & & \\ & & h_{33} & \\ & & & h_{44} \end{array} \right] H=⎣⎢⎢⎡h11h22h33h44⎦⎥⎥⎤
那么水平精度因子（HDOP）、高程精度因子（VDOP）、位置精度因子（PDOP）、钟差精度因子（TDOP）、几何精度因子（GDOP）分别为：
HDOP=h112+h222VDOP=h332PDOP=h112+h222+h332TDOP=h442GDOP=h112+h222+h332+h442\begin{array}{c} HDOP = \sqrt{h_{11}^2 + h_{22}^2} \\ VDOP = \sqrt{h_{33}^2} \\ PDOP = \sqrt{h_{11}^2 + h_{22}^2 + h_{33}^2} \\ TDOP = \sqrt{h_{44}^2} \\ GDOP = \sqrt{h_{11}^2 + h_{22}^2 + h_{33}^2 + h_{44}^2} \end{array} HDOP=h112+h222VDOP=h332PDOP=h112+h222+h332TDOP=h442GDOP=h112+h222+h332+h442
一般GPS接收机在输出定位结果的同时都会输出精度因子，在相同测量误差的情况下，精度因子越小，定位精度越高。

精度因子只与卫星的几何分布有关，有一个简单的方法可以大致判断GDOP的大小：以接收机所在位置为锥顶、以各个卫星所在位置为顶点组成一个锥形体，这个锥形体体积越大，相应的GDOP就越小。