第9讲 单点定位技术理论
9.1 伪距观测值方程
伪距观测值公式表示为：
P r s = ρ r s − c δ t s + c δ t r + I r s + T r s + e P P_r^s = \rho_r^s - c\delta t^s + c\delta t_r + I_r^s + T_r^s + e_P Prs=ρrs−cδts+cδtr+Irs+Trs+eP
其中：
- P r s P_r^s Prs 为卫星 s s s 到接收机 r r r 的伪距观测值
- ρ r s \rho_r^s ρrs 为卫星 s s s 到接收机 r r r 的真实距离
- c c c 为光速
- δ t s \delta t^s δts 为卫星钟差
- δ t r \delta t_r δtr 为接收机钟差
- I r s I_r^s Irs 为电离层延迟
- T r s T_r^s Trs 为对流层延迟
- e P e_P eP 为伪距观测值噪声，单位为米
假设某GNSS接收机接收到来自某卫星的伪距观测值 P r s = 20200000 P_r^s = 20200000 Prs=20200000 米，已知：
- ρ r s = 20185000 \rho_r^s = 20185000 ρrs=20185000 米
- δ t s − δ t r = 50 \delta t^s - \delta t_r = 50 δts−δtr=50 纳秒（1 纳秒 = 1 0 − 9 10^{-9} 10−9 秒）
- I r s = 3 I_r^s = 3 Irs=3 米
- T r s = 2 T_r^s = 2 Trs=2 米
将这些值代入公式：
P r s = ρ r s − c δ t s + c δ t r + I r s + T r s + e P P_r^s = \rho_r^s - c\delta t^s + c\delta t_r + I_r^s + T_r^s + e_P Prs=ρrs−cδts+cδtr+Irs+Trs+eP
其中 c = 299792458 c = 299792458 c=299792458 米/秒，所以：
P r s = 20185000 − 299792458 × 50 × 1 0 − 9 + 3 + 2 + e P P_r^s = 20185000 - 299792458 \times 50 \times 10^{-9} + 3 + 2 + e_P Prs=20185000−299792458×50×10−9+3+2+eP
P r s = 20185000 − 14.989623 + 3 + 2 + e P P_r^s = 20185000 - 14.989623 + 3 + 2 + e_P Prs=20185000−14.989623+3+2+eP
假设噪声 e P e_P eP 很小，可以忽略不计：
P r s ≈ 20185000 − 14.99 + 3 + 2 = 20184990.01 米 P_r^s \approx 20185000 - 14.99 + 3 + 2 = 20184990.01 \text{米} Prs≈20185000−14.99+3+2=20184990.01米
因此，计算得到的伪距观测值约为20184990.01米，接近接收到的20200000米。
通过以上例子，我们可以看到如何利用伪距观测值方程计算接收机和卫星之间的距离，并考虑各种误差因素，如钟差、电离层延迟和对流层延迟。
9.2 定位方程
对于伪距观测值：
P i s = ( X s − X r ) 2 + ( Y s − Y r ) 2 + ( Z s − Z r ) 2 P_i^s = \sqrt{(X^s - X_r)^2 + (Y^s - Y_r)^2 + (Z^s - Z_r)^2} Pis=(Xs−Xr)2+(Ys−Yr)2+(Zs−Zr)2
如果我们令卫星位置向量 P s = ( X s , Y s , Z s ) P^s = (X^s, Y^s, Z^s) Ps=(Xs,Ys,Zs) 接收机位置向量 P r = ( X r , Y r , Z r ) P_r = (X_r, Y_r, Z_r) Pr=(Xr,Yr,Zr) 向量差 Δ P = P s − P r \Delta P = P^s - P_r ΔP=Ps−Pr 向量 Δ P \Delta P ΔP 方向上的单位向量，即为 u \mathbf{u} u。那么有：
ρ i s = Δ P ⋅ u \rho_i^s = \Delta P \cdot \mathbf{u} ρis=ΔP⋅u
其中 u \mathbf{u} u 为 Δ P \Delta P ΔP 的单位向量。
如果我们已知接收机的概略位置为 ( X 0 , Y 0 , Z 0 ) (X_0, Y_0, Z_0) (X0,Y0,Z0)，有：
X r = X 0 + d x X_r = X_0 + dx Xr=X0+dx
Y r = Y 0 + d y Y_r = Y_0 + dy Yr=Y0+dy
Z r = Z 0 + d z Z_r = Z_0 + dz Zr=Z0+dz
即有：
P r = P r 0 + Δ P P_r = P_{r_0} + \Delta P Pr=Pr0+ΔP
Δ P = P s − P r 0 − Δ P \Delta P = P^s - P_{r_0} - \Delta P ΔP=Ps−Pr0−ΔP
其中 P r 0 P_{r_0} Pr0 为接收机概略位置向量， Δ P \Delta P ΔP 为概略位置偏差
ρ i s = ( P s − P r 0 − Δ P ) ⋅ u \rho_i^s = (P^s - P_{r_0} - \Delta P) \cdot \mathbf{u} ρis=(Ps−Pr0−ΔP)⋅u
同时我们计算卫星到接收机概略位置的距离，理论上来说如果概略位置偏差不太大，那么单位向量的差异就不大，甚至可以忽略。
u 0 = ( X s − X 0 R 0 s , Y s − Y 0 R 0 s , Z s − Z 0 R 0 s ) \mathbf{u}_0 = \left( \frac{X^s - X_0}{R_0^s}, \frac{Y^s - Y_0}{R_0^s}, \frac{Z^s - Z_0}{R_0^s} \right) u0=(R0sXs−X0,R0sYs−Y0,R0sZs−Z0)
ρ i s = ( P s − P r 0 − Δ P ) ⋅ u 0 = ( P s − P r 0 ) ⋅ u 0 − Δ P ⋅ u 0 \rho_i^s = (P^s - P_{r_0} - \Delta P) \cdot \mathbf{u}_0 = (P^s - P_{r_0}) \cdot \mathbf{u}_0 - \Delta P \cdot \mathbf{u}_0 ρis=(Ps−Pr0−ΔP)⋅u0=(Ps−Pr0)⋅u0−ΔP⋅u0
进一步简化后：
ρ i s = R 0 s − X s − X 0 R 0 s d x − Y s − Y 0 R 0 s d y − Z s − Z 0 R 0 s d z \rho_i^s = R_0^s - \frac{X^s - X_0}{R_0^s}dx - \frac{Y^s - Y_0}{R_0^s}dy - \frac{Z^s - Z_0}{R_0^s}dz ρis=R0s−R0sXs−X0dx−R0sYs−Y0dy−R0sZs−Z0dz
其中：
R 0 s = ( X s − X 0 ) 2 + ( Y s − Y 0 ) 2 + ( Z s − Z 0 ) 2 R_0^s = \sqrt{(X^s - X_0)^2 + (Y^s - Y_0)^2 + (Z^s - Z_0)^2} R0s=(Xs−X0)2+(Ys−Y0)2+(Zs−Z0)2
这样就将待估参数由接收机位置，变成了相对于接收机概略位置的偏量 (dx, dy, dz)。
进一步整理以下公式：
P i s = ( X s − X 0 + d x ) 2 + ( Y s − Y 0 + d y ) 2 + ( Z s − Z 0 + d z ) 2 − c δ t + I i s − T i s − ϵ P i s P_i^s = \sqrt{(X^s - X_0 + dx)^2 + (Y^s - Y_0 + dy)^2 + (Z^s - Z_0 + dz)^2} - c\delta t + I_i^s - T_i^s - \epsilon_{P_i^s} Pis=(Xs−X0+dx)2+(Ys−Y0+dy)2+(Zs−Z0+dz)2−cδt+Iis−Tis−ϵPis
如果我们使用电离层和对流层模型对大气误差进行估计，并使用广播星历计算卫星位置和卫星钟改正，并在公式中扣除：
P i s = ( X s − X 0 ) 2 + ( Y s − Y 0 ) 2 + ( Z s − Z 0 ) 2 + c δ t − I i s − T i s − ϵ P i s − X s − X 0 R 0 s d x − Y s − Y 0 R 0 s d y − Z s − Z 0 R 0 s d z P_i^s = \sqrt{(X^s - X_0)^2 + (Y^s - Y_0)^2 + (Z^s - Z_0)^2} + c\delta t - I_i^s - T_i^s - \epsilon_{P_i^s} - \frac{X^s - X_0}{R_0^s}dx - \frac{Y^s - Y_0}{R_0^s}dy - \frac{Z^s - Z_0}{R_0^s}dz Pis=(Xs−X0)2+(Ys−Y0)2+(Zs−Z0)2+cδt−Iis−Tis−ϵPis−R0sXs−X0dx−R0sYs−Y0dy−R0sZs−Z0dz
那么整理可得：
P i s = l i s − m i s d x − n i s d y − p i s d z + c δ t P_i^s = l_i^s - m_i^s dx - n_i^s dy - p_i^s dz + c\delta t Pis=lis−misdx−nisdy−pisdz+cδt
其中：
l i s = P i s − ( X s − X 0 ) 2 + ( Y s − Y 0 ) 2 + ( Z s − Z 0 ) 2 + c δ t − I i s − T i s − ϵ P i s l_i^s = P_i^s - \sqrt{(X^s - X_0)^2 + (Y^s - Y_0)^2 + (Z^s - Z_0)^2} + c\delta t - I_i^s - T_i^s - \epsilon_{P_i^s} lis=Pis−(Xs−X0)2+(Ys−Y0)2+(Zs−Z0)2+cδt−Iis−Tis−ϵPis 称为先验残差。
m i s = X s − X 0 R 0 s , n i s = Y s − Y 0 R 0 s , p i s = Z s − Z 0 R 0 s m_i^s = \frac{X^s - X_0}{R_0^s}, n_i^s = \frac{Y^s - Y_0}{R_0^s}, p_i^s = \frac{Z^s - Z_0}{R_0^s} mis=R0sXs−X0,nis=R0sYs−Y0,pis=R0sZs−Z0 为从概略位置到卫星的方向余弦。
如果可以观测到同一系统的四个卫星，就可以估计出四个未知状态量。
{ l 1 s = l 1 s − m 1 s d x − n 1 s d y − p 1 s d z + c δ t l 2 s = l 2 s − m 2 s d x − n 2 s d y − p 2 s d z + c δ t l 3 s = l 3 s − m 3 s d x − n 3 s d y − p 3 s d z + c δ t l 4 s = l 4 s − m 4 s d x − n 4 s d y − p 4 s d z + c δ t \begin{cases} l_1^s = l_1^s - m_1^s dx - n_1^s dy - p_1^s dz + c\delta t \\ l_2^s = l_2^s - m_2^s dx - n_2^s dy - p_2^s dz + c\delta t \\ l_3^s = l_3^s - m_3^s dx - n_3^s dy - p_3^s dz + c\delta t \\ l_4^s = l_4^s - m_4^s dx - n_4^s dy - p_4^s dz + c\delta t \end{cases} ⎩ ⎨ ⎧l1s=l1s−m1sdx−n1sdy−p1sdz+cδtl2s=l2s−m2sdx−n2sdy−p2sdz+cδtl3s=l3s−m3sdx−n3sdy−p3sdz+cδtl4s=l4s−m4sdx−n4sdy−p4sdz+cδt
整理成矩阵形式，并用残差值表示：
V = [ l 1 s l 2 s l 3 s l 4 s ] = [ m 1 s n 1 s p 1 s − 1 m 2 s n 2 s p 2 s − 1 m 3 s n 3 s p 3 s − 1 m 4 s n 4 s p 4 s − 1 ] ⋅ δ z = A ⋅ δ z V = \begin{bmatrix} l_1^s \\ l_2^s \\ l_3^s \\ l_4^s \end{bmatrix} = \begin{bmatrix} m_1^s & n_1^s & p_1^s & -1 \\ m_2^s & n_2^s & p_2^s & -1 \\ m_3^s & n_3^s & p_3^s & -1 \\ m_4^s & n_4^s & p_4^s & -1 \end{bmatrix} \cdot \delta z = A \cdot \delta z V= l1sl2sl3sl4s = m1sm2sm3sm4sn1sn2sn3sn4sp1sp2sp3sp4s−1−1−1−1 ⋅δz=A⋅δz
即有以上公式：
V = − A ⋅ δ z V = -A \cdot \delta z V=−A⋅δz
我们将 V V V 称为先验残差矩阵， A A A 为设计矩阵或称为jacobian矩阵， δ z \delta z δz 为待估状态量。
以上只是单独一个系统，比如仅仅为GPS定位，那如果我增加北斗系统，那方程该文如何表示。
实例
假设我们有以下四个卫星的伪距观测值 P 1 , P 2 , P 3 , P 4 P_1, P_2, P_3, P_4 P1,P2,P3,P4，以及它们的坐标 ( X 1 , Y 1 , Z 1 ) , ( X 2 , Y 2 , Z 2 ) , ( X 3 , Y 3 , Z 3 ) , ( X 4 , Y 4 , Z 4 ) (X_1, Y_1, Z_1), (X_2, Y_2, Z_2), (X_3, Y_3, Z_3), (X_4, Y_4, Z_4) (X1,Y1,Z1),(X2,Y2,Z2),(X3,Y3,Z3),(X4,Y4,Z4) 和接收机的概略位置 ( X 0 , Y 0 , Z 0 ) (X_0, Y_0, Z_0) (X0,Y0,Z0)。
计算每个卫星到接收机概略位置的距离：
计算从接收机的概略位置到每个卫星的距离，公式如下：
R 0 1 = ( X 1 − X 0 ) 2 + ( Y 1 − Y 0 ) 2 + ( Z 1 − Z 0 ) 2 R_0^1 = \sqrt{(X_1 - X_0)^2 + (Y_1 - Y_0)^2 + (Z_1 - Z_0)^2} R01=(X1−X0)2+(Y1−Y0)2+(Z1−Z0)2
R 0 2 = ( X 2 − X 0 ) 2 + ( Y 2 − Y 0 ) 2 + ( Z 2 − Z 0 ) 2 R_0^2 = \sqrt{(X_2 - X_0)^2 + (Y_2 - Y_0)^2 + (Z_2 - Z_0)^2} R02=(X2−X0)2+(Y2−Y0)2+(Z2−Z0)2
R 0 3 = ( X 3 − X 0 ) 2 + ( Y 3 − Y 0 ) 2 + ( Z 3 − Z 0 ) 2 R_0^3 = \sqrt{(X_3 - X_0)^2 + (Y_3 - Y_0)^2 + (Z_3 - Z_0)^2} R03=(X3−X0)2+(Y3−Y0)2+(Z3−Z0)2
R 0 4 = ( X 4 − X 0 ) 2 + ( Y 4 − Y 0 ) 2 + ( Z 4 − Z 0 ) 2 R_0^4 = \sqrt{(X_4 - X_0)^2 + (Y_4 - Y_0)^2 + (Z_4 - Z_0)^2} R04=(X4−X0)2+(Y4−Y0)2+(Z4−Z0)2
这些距离表示从概略位置到每个卫星的几何距离。
计算方向余弦：
方向余弦用于表示从接收机概略位置到每个卫星的方向向量。计算如下：
m 1 = X 1 − X 0 R 0 1 , n 1 = Y 1 − Y 0 R 0 1 , p 1 = Z 1 − Z 0 R 0 1 m_1 = \frac{X_1 - X_0}{R_0^1}, \quad n_1 = \frac{Y_1 - Y_0}{R_0^1}, \quad p_1 = \frac{Z_1 - Z_0}{R_0^1} m1=R01X1−X0,n1=R01Y1−Y0,p1=R01Z1−Z0
m 2 = X 2 − X 0 R 0 2 , n 2 = Y 2 − Y 0 R 0 2 , p 2 = Z 2 − Z 0 R 0 2 m_2 = \frac{X_2 - X_0}{R_0^2}, \quad n_2 = \frac{Y_2 - Y_0}{R_0^2}, \quad p_2 = \frac{Z_2 - Z_0}{R_0^2} m2=R02X2−X0,n2=R02Y2−Y0,p2=R02Z2−Z0
m 3 = X 3 − X 0 R 0 3 , n 3 = Y 3 − Y 0 R 0 3 , p 3 = Z 3 − Z 0 R 0 3 m_3 = \frac{X_3 - X_0}{R_0^3}, \quad n_3 = \frac{Y_3 - Y_0}{R_0^3}, \quad p_3 = \frac{Z_3 - Z_0}{R_0^3} m3=R03X3−X0,n3=R03Y3−Y0,p3=R03Z3−Z0
m 4 = X 4 − X 0 R 0 4 , n 4 = Y 4 − Y 0 R 0 4 , p 4 = Z 4 − Z 0 R 0 4 m_4 = \frac{X_4 - X_0}{R_0^4}, \quad n_4 = \frac{Y_4 - Y_0}{R_0^4}, \quad p_4 = \frac{Z_4 - Z_0}{R_0^4} m4=R04X4−X0,n4=R04Y4−Y0,p4=R04Z4−Z0
这些方向余弦描述了从接收机概略位置到每个卫星的单位方向向量。
形成设计矩阵 A A A 和先验残差矩阵 V V V：
设计矩阵 A A A 和先验残差矩阵 V V V 用于表示伪距观测方程的线性形式：
A = [ m 1 n 1 p 1 − 1 m 2 n 2 p 2 − 1 m 3 n 3 p 3 − 1 m 4 n 4 p 4 − 1 ] A = \begin{bmatrix} m_1 & n_1 & p_1 & -1 \\ m_2 & n_2 & p_2 & -1 \\ m_3 & n_3 & p_3 & -1 \\ m_4 & n_4 & p_4 & -1 \end{bmatrix} A= m1m2m3m4n1n2n3n4p1p2p3p4−1−1−1−1
先验残差矩阵 $ V $ 计算如下：
V = [ l 1 l 2 l 3 l 4 ] = [ P 1 − R 0 1 + c δ t − I 1 − T 1 − ϵ P 1 P 2 − R 0 2 + c δ t − I 2 − T 2 − ϵ P 2 P 3 − R 0 3 + c δ t − I 3 − T 3 − ϵ P 3 P 4 − R 0 4 + c δ t − I 4 − T 4 − ϵ P 4 ] V = \begin{bmatrix} l_1 \\ l_2 \\ l_3 \\ l_4 \end{bmatrix} = \begin{bmatrix} P_1 - R_0^1 + c\delta t - I_1 - T_1 - \epsilon_{P_1} \\ P_2 - R_0^2 + c\delta t - I_2 - T_2 - \epsilon_{P_2} \\ P_3 - R_0^3 + c\delta t - I_3 - T_3 - \epsilon_{P_3} \\ P_4 - R_0^4 + c\delta t - I_4 - T_4 - \epsilon_{P_4} \end{bmatrix} V= l1l2l3l4 = P1−R01+cδt−I1−T1−ϵP1P2−R02+cδt−I2−T2−ϵP2P3−R03+cδt−I3−T3−ϵP3P4−R04+cδt−I4−T4−ϵP4
这里的 l i l_i li 表示每个伪距观测值减去计算得到的几何距离并进行误差修正后的值。
解方程 V = − A ⋅ δ z V = -A \cdot \delta z V=−A⋅δz 以估计状态量 δ z = [ d x d y d z c δ t ] T \delta z = \begin{bmatrix} dx & dy & dz & c\delta t \end{bmatrix}^T δz=[dxdydzcδt]T：
通过求解线性方程组 V = − A ⋅ δ z V = -A \cdot \delta z V=−A⋅δz，我们可以估计状态量 δ z \delta z δz，即接收机位置的修正量和接收机钟差：
δ z = ( A T A ) − 1 A T V \delta z = (A^T A)^{-1} A^T V δz=(ATA)−1ATV
这里， ( A T A ) − 1 A T (A^T A)^{-1} A^T (ATA)−1AT 是伪逆矩阵，用于求解最小二乘问题。
更新接收机位置：
根据估计的修正量，更新接收机的位置：
X r = X 0 + d x , Y r = Y 0 + d y , Z r = Z 0 + d z X_r = X_0 + dx, \quad Y_r = Y_0 + dy, \quad Z_r = Z_0 + dz Xr=X0+dx,Yr=Y0+dy,Zr=Z0+dz
最终，我们得到了更精确的接收机位置 ( X r , Y r , Z r ) (X_r, Y_r, Z_r) (Xr,Yr,Zr)。
通过以上步骤，可以得到接收机的精确位置偏差，并更新接收机的概略位置，从而得到更精确的定位结果。
9.3 多系统定位方程
由于GPS和BDS是两个系统，所以主要表现在接收机端的误差会存在差异，这些误差会被接收机钟进行吸收，所以我们一般可以将GPS和BDS系统分别设计成一个接收机钟差。
即其设计矩阵和待估状态量如下：
A = [ G 1 m G 1 n G 1 p G 1 − 1 0 G 2 m G 2 n G 2 p G 2 − 1 0 G 3 m G 3 n G 3 p G 3 − 1 0 G n m G n n G n p G n − 1 0 B 1 m B 1 n B 1 p B 1 0 − 1 B 2 m B 2 n B 2 p B 2 0 − 1 B 3 m B 3 n B 3 p B 3 0 − 1 B n m B n n B n p B n 0 − 1 ] A = \begin{bmatrix} G^1 & m_{G1} & n_{G1} & p_{G1} & -1 & 0 \\ G^2 & m_{G2} & n_{G2} & p_{G2} & -1 & 0 \\ G^3 & m_{G3} & n_{G3} & p_{G3} & -1 & 0 \\ G^n & m_{Gn} & n_{Gn} & p_{Gn} & -1 & 0 \\ B^1 & m_{B1} & n_{B1} & p_{B1} & 0 & -1 \\ B^2 & m_{B2} & n_{B2} & p_{B2} & 0 & -1 \\ B^3 & m_{B3} & n_{B3} & p_{B3} & 0 & -1 \\ B^n & m_{Bn} & n_{Bn} & p_{Bn} & 0 & -1 \end{bmatrix} A= G1G2G3GnB1B2B3BnmG1mG2mG3mGnmB1mB2mB3mBnnG1nG2nG3nGnnB1nB2nB3nBnpG1pG2pG3pGnpB1pB2pB3pBn−1−1−1−100000000−1−1−1−1
δ z = [ d x d y d z c δ t G c δ t C ] \delta z = \begin{bmatrix} dx \\ dy \\ dz \\ c\delta t^G \\ c\delta t^C \end{bmatrix} δz= dxdydzcδtGcδtC
至于再增加GAL/GLO系统，原理一致，不再展开。
实例
假设我们有以下GPS和BDS两个系统的伪距观测值 P G 1 , P G 2 , P G 3 , P G n , P B 1 , P B 2 , P B 3 , P B n P_{G1}, P_{G2}, P_{G3}, P_{Gn}, P_{B1}, P_{B2}, P_{B3}, P_{Bn} PG1,PG2,PG3,PGn,PB1,PB2,PB3,PBn，以及它们的坐标 ( X G 1 , Y G 1 , Z G 1 ) , ( X G 2 , Y G 2 , Z G 2 ) , ( X G 3 , Y G 3 , Z G 3 ) , ( X G n , Y G n , Z G n ) , ( X B 1 , Y B 1 , Z B 1 ) , ( X B 2 , Y B 2 , Z B 2 ) , ( X B 3 , Y B 3 , Z B 3 ) , ( X B n , Y B n , Z B n ) (X_{G1}, Y_{G1}, Z_{G1}), (X_{G2}, Y_{G2}, Z_{G2}), (X_{G3}, Y_{G3}, Z_{G3}), (X_{Gn}, Y_{Gn}, Z_{Gn}), (X_{B1}, Y_{B1}, Z_{B1}), (X_{B2}, Y_{B2}, Z_{B2}), (X_{B3}, Y_{B3}, Z_{B3}), (X_{Bn}, Y_{Bn}, Z_{Bn}) (XG1,YG1,ZG1),(XG2,YG2,ZG2),(XG3,YG3,ZG3),(XGn,YGn,ZGn),(XB1,YB1,ZB1),(XB2,YB2,ZB2),(XB3,YB3,ZB3),(XBn,YBn,ZBn) 和接收机的概略位置 ( X 0 , Y 0 , Z 0 ) (X_0, Y_0, Z_0) (X0,Y0,Z0)。
计算每个卫星到接收机概略位置的距离：
计算从接收机的概略位置到每个卫星的距离，公式如下：
R G 0 1 = ( X G 1 − X 0 ) 2 + ( Y G 1 − Y 0 ) 2 + ( Z G 1 − Z 0 ) 2 R_{G0}^1 = \sqrt{(X_{G1} - X_0)^2 + (Y_{G1} - Y_0)^2 + (Z_{G1} - Z_0)^2} RG01=(XG1−X0)2+(YG1−Y0)2+(ZG1−Z0)2
R G 0 2 = ( X G 2 − X 0 ) 2 + ( Y G 2 − Y 0 ) 2 + ( Z G 2 − Z 0 ) 2 R_{G0}^2 = \sqrt{(X_{G2} - X_0)^2 + (Y_{G2} - Y_0)^2 + (Z_{G2} - Z_0)^2} RG02=(XG2−X0)2+(YG2−Y0)2+(ZG2−Z0)2
R G 0 3 = ( X G 3 − X 0 ) 2 + ( Y G 3 − Y 0 ) 2 + ( Z G 3 − Z 0 ) 2 R_{G0}^3 = \sqrt{(X_{G3} - X_0)^2 + (Y_{G3} - Y_0)^2 + (Z_{G3} - Z_0)^2} RG03=(XG3−X0)2+(YG3−Y0)2+(ZG3−Z0)2
R G 0 n = ( X G n − X 0 ) 2 + ( Y G n − Y 0 ) 2 + ( Z G n − Z 0 ) 2 R_{G0}^n = \sqrt{(X_{Gn} - X_0)^2 + (Y_{Gn} - Y_0)^2 + (Z_{Gn} - Z_0)^2} RG0n=(XGn−X0)2+(YGn−Y0)2+(ZGn−Z0)2
R B 0 1 = ( X B 1 − X 0 ) 2 + ( Y B 1 − Y 0 ) 2 + ( Z B 1 − Z 0 ) 2 R_{B0}^1 = \sqrt{(X_{B1} - X_0)^2 + (Y_{B1} - Y_0)^2 + (Z_{B1} - Z_0)^2} RB01=(XB1−X0)2+(YB1−Y0)2+(ZB1−Z0)2
R B 0 2 = ( X B 2 − X 0 ) 2 + ( Y B 2 − Y 0 ) 2 + ( Z B 2 − Z 0 ) 2 R_{B0}^2 = \sqrt{(X_{B2} - X_0)^2 + (Y_{B2} - Y_0)^2 + (Z_{B2} - Z_0)^2} RB02=(XB2−X0)2+(YB2−Y0)2+(ZB2−Z0)2
R B 0 3 = ( X B 3 − X 0 ) 2 + ( Y B 3 − Y 0 ) 2 + ( Z B 3 − Z 0 ) 2 R_{B0}^3 = \sqrt{(X_{B3} - X_0)^2 + (Y_{B3} - Y_0)^2 + (Z_{B3} - Z_0)^2} RB03=(XB3−X0)2+(YB3−Y0)2+(ZB3−Z0)2
R B 0 n = ( X B n − X 0 ) 2 + ( Y B n − Y 0 ) 2 + ( Z B n − Z 0 ) 2 R_{B0}^n = \sqrt{(X_{Bn} - X_0)^2 + (Y_{Bn} - Y_0)^2 + (Z_{Bn} - Z_0)^2} RB0n=(XBn−X0)2+(YBn−Y0)2+(ZBn−Z0)2
计算方向余弦：
方向余弦用于表示从接收机概略位置到每个卫星的方向向量。计算如下：
m G 1 = X G 1 − X 0 R G 0 1 , n G 1 = Y G 1 − Y 0 R G 0 1 , p G 1 = Z G 1 − Z 0 R G 0 1 m_{G1} = \frac{X_{G1} - X_0}{R_{G0}^1}, \quad n_{G1} = \frac{Y_{G1} - Y_0}{R_{G0}^1}, \quad p_{G1} = \frac{Z_{G1} - Z_0}{R_{G0}^1} mG1=RG01XG1−X0,nG1=RG01YG1−Y0,pG1=RG01ZG1−Z0
m G 2 = X G 2 − X 0 R G 0 2 , n G 2 = Y G 2 − Y 0 R G 0 2 , p G 2 = Z G 2 − Z 0 R G 0 2 m_{G2} = \frac{X_{G2} - X_0}{R_{G0}^2}, \quad n_{G2} = \frac{Y_{G2} - Y_0}{R_{G0}^2}, \quad p_{G2} = \frac{Z_{G2} - Z_0}{R_{G0}^2} mG2=RG02XG2−X0,nG2=RG02YG2−Y0,pG2=RG02ZG2−Z0
m G 3 = X G 3 − X 0 R G 0 3 , n G 3 = Y G 3 − Y 0 R G 0 3 , p G 3 = Z G 3 − Z 0 R G 0 3 m_{G3} = \frac{X_{G3} - X_0}{R_{G0}^3}, \quad n_{G3} = \frac{Y_{G3} - Y_0}{R_{G0}^3}, \quad p_{G3} = \frac{Z_{G3} - Z_0}{R_{G0}^3} mG3=RG03XG3−X0,nG3=RG03YG3−Y0,pG3=RG03ZG3−Z0
m G n = X G n − X 0 R G 0 n , n G n = Y G n − Y 0 R G 0 n , p G n = Z G n − Z 0 R G 0 n m_{Gn} = \frac{X_{Gn} - X_0}{R_{G0}^n}, \quad n_{Gn} = \frac{Y_{Gn} - Y_0}{R_{G0}^n}, \quad p_{Gn} = \frac{Z_{Gn} - Z_0}{R_{G0}^n} mGn=RG0nXGn−X0,nGn=RG0nYGn−Y0,pGn=RG0nZGn−Z0
m B 1 = X B 1 − X 0 R B 0 1 , n B 1 = Y B 1 − Y 0 R B 0 1 , p B 1 = Z B 1 − Z 0 R B 0 1 m_{B1} = \frac{X_{B1} - X_0}{R_{B0}^1}, \quad n_{B1} = \frac{Y_{B1} - Y_0}{R_{B0}^1}, \quad p_{B1} = \frac{Z_{B1} - Z_0}{R_{B0}^1} mB1=RB01XB1−X0,nB1=RB01YB1−Y0,pB1=RB01ZB1−Z0
m B 2 = X B 2 − X 0 R B 0 2 , n B 2 = Y B 2 − Y 0 R B 0 2 , p B 2 = Z B 2 − Z 0 R B 0 2 m_{B2} = \frac{X_{B2} - X_0}{R_{B0}^2}, \quad n_{B2} = \frac{Y_{B2} - Y_0}{R_{B0}^2}, \quad p_{B2} = \frac{Z_{B2} - Z_0}{R_{B0}^2} mB2=RB02XB2−X0,nB2=RB02YB2−Y0,pB2=RB02ZB2−Z0
m B 3 = X B 3 − X 0 R B 0 3 , n B 3 = Y B 3 − Y 0 R B 0 3 , p B 3 = Z B 3 − Z 0 R B 0 3 m_{B3} = \frac{X_{B3} - X_0}{R_{B0}^3}, \quad n_{B3} = \frac{Y_{B3} - Y_0}{R_{B0}^3}, \quad p_{B3} = \frac{Z_{B3} - Z_0}{R_{B0}^3} mB3=RB03XB3−X0,nB3=RB03YB3−Y0,pB3=RB03ZB3−Z0
m B n = X B n − X 0 R B 0 n , n B n = Y B n − Y 0 R B 0 n , p B n = Z B n − Z 0 R B 0 n m_{Bn} = \frac{X_{Bn} - X_0}{R_{B0}^n}, \quad n_{Bn} = \frac{Y_{Bn} - Y_0}{R_{B0}^n}, \quad p_{Bn} = \frac{Z_{Bn} - Z_0}{R_{B0}^n} mBn=RB0nXBn−X0,nBn=RB0nYBn−Y0,pBn=RB0nZBn−Z0
形成设计矩阵 A A A 和先验残差矩阵 V V V：
设计矩阵 A A A 和先验残差矩阵 V V V 用于表示伪距观测方程的线性形式：
A = [ m G 1 n G 1 p G 1 − 1 0 m G 2 n G 2 p G 2 − 1 0 m G 3 n G 3 p G 3 − 1 0 m G n n G n p G n − 1 0 m B 1 n B 1 p B 1 0 − 1 m B 2 n B 2 p B 2 0 − 1 m B 3 n B 3 p B 3 0 − 1 m B n n B n p B n 0 − 1 ] A = \begin{bmatrix} m_{G1} & n_{G1} & p_{G1} & -1 & 0 \\ m_{G2} & n_{G2} & p_{G2} & -1 & 0 \\ m_{G3} & n_{G3} & p_{G3} & -1 & 0 \\ m_{Gn} & n_{Gn} & p_{Gn} & -1 & 0 \\ m_{B1} & n_{B1} & p_{B1} & 0 & -1 \\ m_{B2} & n_{B2} & p_{B2} & 0 & -1 \\ m_{B3} & n_{B3} & p_{B3} & 0 & -1 \\ m_{Bn} & n_{Bn} & p_{Bn} & 0 & -1 \end{bmatrix} A= mG1mG2mG3mGnmB1mB2mB3mBnnG1nG2nG3nGnnB1nB2nB3nBnpG1pG2pG3pGnpB1pB2pB3pBn−1−1−1−100000000−1−1−1−1
先验残差矩阵 V V V 计算如下：
V = [ l G 1 l G 2 l G 3 l G n l B 1 l B 2 l B 3 l B n ] = [ P G 1 − R G 0 1 + c δ t G − I G 1 − T G 1 − ϵ P G 1 P G 2 − R G 0 2 + c δ t G − I G 2 − T G 2 − ϵ P G 2 P G 3 − R G 0 3 + c δ t G − I G 3 − T G 3 − ϵ P G 3 P G n − R G 0 n + c δ t G − I G n − T G n − ϵ P G n P B 1 − R B 0 1 + c δ t C − I B 1 − T B 1 − ϵ P B 1 P B 2 − R B 0 2 + c δ t C − I B 2 − T B 2 − ϵ P B 2 P B 3 − R B 0 3 + c δ t C − I B 3 − T B 3 − ϵ P B 3 P B n − R B 0 n + c δ t C − I B n − T B n − ϵ P B n ] V = \begin{bmatrix} l_{G1} \\ l_{G2} \\ l_{G3} \\ l_{Gn} \\ l_{B1} \\ l_{B2} \\ l_{B3} \\ l_{Bn} \end{bmatrix} = \begin{bmatrix} P_{G1} - R_{G0}^1 + c\delta t^G - I_{G1} - T_{G1} - \epsilon_{P_{G1}} \\ P_{G2} - R_{G0}^2 + c\delta t^G - I_{G2} - T_{G2} - \epsilon_{P_{G2}} \\ P_{G3} - R_{G0}^3 + c\delta t^G - I_{G3} - T_{G3} - \epsilon_{P_{G3}} \\ P_{Gn} - R_{G0}^n + c\delta t^G - I_{Gn} - T_{Gn} - \epsilon_{P_{Gn}} \\ P_{B1} - R_{B0}^1 + c\delta t^C - I_{B1} - T_{B1} - \epsilon_{P_{B1}} \\ P_{B2} - R_{B0}^2 + c\delta t^C - I_{B2} - T_{B2} - \epsilon_{P_{B2}} \\ P_{B3} - R_{B0}^3 + c\delta t^C - I_{B3} - T_{B3} - \epsilon_{P_{B3}} \\ P_{Bn} - R_{B0}^n + c\delta t^C - I_{Bn} - T_{Bn} - \epsilon_{P_{Bn}} \end{bmatrix} V= lG1lG2lG3lGnlB1lB2lB3lBn = PG1−RG01+cδtG−IG1−TG1−ϵPG1PG2−RG02+cδtG−IG2−TG2−ϵPG2PG3−RG03+cδtG−IG3−TG3−ϵPG3PGn−RG0n+cδtG−IGn−TGn−ϵPGnPB1−RB01+cδtC−IB1−TB1−ϵPB1PB2−RB02+cδtC−IB2−TB2−ϵPB2PB3−RB03+cδtC−IB3−TB3−ϵPB3PBn−RB0n+cδtC−IBn−TBn−ϵPBn
这里的 l i l_i li 表示每个伪距观测值减去计算得到的几何距离并进行误差修正后的值。
解方程 V = − A ⋅ δ z V = -A \cdot \delta z V=−A⋅δz 以估计状态量 δ z = [ d x d y d z c δ t G c δ t C ] T \delta z = \begin{bmatrix} dx & dy & dz & c\delta t^G & c\delta t^C \end{bmatrix}^T δz=[dxdydzcδtGcδtC]T：
通过求解线性方程组 V = − A ⋅ δ z V = -A \cdot \delta z V=−A⋅δz，我们可以估计状态量 δ z \delta z δz，即接收机位置的修正量和接收机钟差：
δ z = ( A T A ) − 1 A T V \delta z = (A^T A)^{-1} A^T V δz=(ATA)−1ATV
这里， ( A T A ) − 1 A T (A^T A)^{-1} A^T (ATA)−1AT 是伪逆矩阵，用于求解最小二乘问题。
更新接收机位置：
根据估计的修正量，更新接收机的位置：
X r = X 0 + d x , Y r = Y 0 + d y , Z r = Z 0 + d z X_r = X_0 + dx, \quad Y_r = Y_0 + dy, \quad Z_r = Z_0 + dz Xr=X0+dx,Yr=Y0+dy,Zr=Z0+dz
最终，我们得到了更精确的接收机位置 ( X r , Y r , Z r ) (X_r, Y_r, Z_r) (Xr,Yr,Zr)。
通过以上步骤，可以得到接收机的精确位置偏差，并更新接收机的概略位置，从而得到更精确的定位结果。
9.4 定位流程
