GPS从入门到放弃(十) — 定位方程解算和定位精度
上一篇伪距与载波相位中我们介绍了伪距的计算方法,也得到了包含 ( x , y , z , δ t ) (x,\ y,\ z,\ \delta_t) (x, y, z, δt) 四个未知数的GPS定位基本方程:
( x − x s ) 2 + ( y − y s ) 2 + ( z − z s ) 2 + c ⋅ δ t = ρ + c ⋅ δ t , s − c I − c T − 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ϵ
那么根据这个方程我们怎么来定位呢?
这个方程中的 I I I 和 T T T 分别是大气电离层导致的延时和大气对流层导致的延时,这些延时的计算方法放到后面再讲,目前我们先把它当作已知量。 δ t , s \delta_{t,s} δt,s 为卫星钟差,在导航电文中有参数修正,以后再讲。于是这个方程中只涉及到四个未知数和一个误差。我们先考虑简单的情况,即暂时不管误差 ϵ \epsilon ϵ ,在分析定位精度的时候再来考虑它。
根据我们第一篇GPS基础原理讲过GPS的基本原理,只需已知四颗卫星的测量值,即可组成一个四元方程组,然后解出来这四个未知数。要注意的是这个方程组是一个非线性方程组,因此在实际解算过程中,常用牛顿迭代法来进行。
牛顿迭代法
牛顿迭代法是一个常用的解非线性方程组的方法,它将非线性方程组在一个估计解的附近进行线性化,然后求解线性化后的方程组,接着再更新解的估计值。如此反复迭代,直到解的精度满足要求为止。
根据牛顿迭代法,将四元方程组在第k次迭代的估计解 [ x k y k z k δ t , k ] T [x_k \ \ y_k \ \ z_k\ \ \delta_{t,k}]^T [xk yk zk δt,k]T 处线性化后方程组为:
G [ Δ x Δ y Δ z c ⋅ Δ δ 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 = [ x k − x s , 1 ( x k − x s , 1 ) 2 + ( y k − y s , 1 ) 2 + ( z k − z s , 1 ) 2 y k − y s , 1 ( x k − x s , 1 ) 2 + ( y k − y s , 1 ) 2 + ( z k − z s , 1 ) 2 z k − z s , 1 ( x k − x s , 1 ) 2 + ( y k − y s , 1 ) 2 + ( z k − z s , 1 ) 2 1 x k − x s , 2 ( x k − x s , 2 ) 2 + ( y k − y s , 2 ) 2 + ( z k − z s , 2 ) 2 y k − y s , 2 ( x k − x s , 2 ) 2 + ( y k − y s , 2 ) 2 + ( z k − z s , 2 ) 2 z k − z s , 2 ( x k − x s , 2 ) 2 + ( y k − y s , 2 ) 2 + ( z k − z s , 2 ) 2 1 x k − x s , 3 ( x k − x s , 3 ) 2 + ( y k − y s , 3 ) 2 + ( z k − z s , 3 ) 2 y k − y s , 3 ( x k − x s , 3 ) 2 + ( y k − y s , 3 ) 2 + ( z k − z s , 3 ) 2 z k − z s , 3 ( x k − x s , 3 ) 2 + ( y k − y s , 3 ) 2 + ( z k − z s , 3 ) 2 1 x k − x s , 4 ( x k − x s , 4 ) 2 + ( y k − y s , 4 ) 2 + ( z k − z s , 4 ) 2 y k − y s , 4 ( x k − x s , 4 ) 2 + ( y k − y s , 4 ) 2 + ( z k − z s , 4 ) 2 z k − z s , 4 ( x k − x s , 4 ) 2 + ( y k − y s , 4 ) 2 + ( z k − z s , 4 ) 2 1 ] \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)2