单目测距代码 python

文章目录

• 测距码测定卫地距

• 观测方程
• 程序实现步骤：

测距码测定卫地距

原理：首先架设卫星钟和接收机钟均无误差，都能与标准的GPS时间保持严格同步。在某一时刻t，卫星在卫星钟的控制下发出某一结构的测距码，与此同时，接收机则在接收机钟的控制下产生相同的测距码。由卫星所产生的复制码经$\varDelta t$时间的传播后到达接收机并被接收机所接收。由接收机所产生的复制码则经过一个时间延迟器延迟时间$\tau$后与接收到的卫星信号进行比对。如果这两个信号尚未对齐，就调整延迟时间$\tau$，直至这两个信号对齐为止。此时，复制码的延迟时间$\tau$就等于卫星信号的传播时间$\varDelta t$，将其乘以真空中的光速$c$ 后即可得到卫地间的伪距$\rho$：
$\rho = \tau * c = \varDelta t * c$
由于卫星钟和接收机钟实际上不可避免地存在误差，此外还有电离层、对流层等的影响。求得的$\rho$并不等于卫星与测站的真实距离，故称为伪距。

观测方程

在伪距测量中，直接量测值是信号到达接收机的时刻tr与信号离开卫星的时刻（卫星钟量测）之差，此差值与真空中的光速c乘积即为观测值p，即：
$\rho _{r}^{s}=c\cdot \left( t_{r}^{}-t_{s}^{} \right)$
但实际上，卫星钟与接收机钟均有误差
$t_{r}^{$
$t_{s}^{$
代入得:
$R_{r}^{s}=c\cdot \left( t_{r}^{$
加上电离层、对流层、多路径等各种带来的误差：
$R_{r}^{s}=\rho _{r}^{s}+c\cdot \left( \delta t_r-\delta t_s \right)+\delta _{\varSigma}$
$R^{s}_{r}$ 表示伪距，伪距=真实距离+误差距离，加上电离层、对流层带来的误差
根据卫星与测站坐标，卫星$(x_{i},y_{i},z_{i})$与测站间$(x,y,z)$几何距离为
$\rho_{r}^{s}=\sqrt{\left( x_i-x \right) ^2+\left( y_i-y \right) ^2+\left( z_i-z \right) ^2}$
$R_{r}^{s}=\sqrt{\left( x_i-x \right) ^2+\left( y_i-y \right) ^2+\left( z_i-z \right) ^2}+c\cdot \left( \delta t_r-\delta t_s \right)+\delta _{\varSigma}$
如何求解出方程呢？式中存在未知数$(x,y,z)$和$\delta t_r$ ，卫星位置$(x_{i},y_{i},z_{i})$、卫星钟差$\delta t_s$是可以通过广播星历得到的，一般伪距定位不考虑电离层等误差改正。故方程中存在四个未知数，欲求解出测站坐标，至少需要四个方程，即在测站位置需同时接收到4颗及以上的卫星，才能对方程进行求解。为方便方程的解算，通常需对伪距方程进行线性化，在多数情况下，接收到的卫星数量会大于4颗，一般采用最小二乘法对方程进行解算。
对$\rho_{r}^{s}=\sqrt{\left( x_i-x \right) ^2+\left( y_i-y \right) ^2+\left( z_i-z \right) ^2}$在$x=\left( x_{r}^{0},y_{r}^{0},z_{r}^{0} \right)$进行泰勒展开，
$\begin{cases} \rho =F\left( x_0 \right) +\frac{\partial F\left( x \right)}{\partial x}\mid_{x_0}^{}dx+\varepsilon\\ dx=x-x_0=\left[ \begin{array}{c} \varDelta x\\ \varDelta y\\ \varDelta z\\\end{array} \right]\\ \,\,\frac{\partial F\left( x \right)}{\partial x}=\left( \frac{\partial F}{\partial x_r},\frac{\partial F}{\partial y_r},\frac{\partial F}{\partial z_r} \right)\\\end{cases}$
代入$R_{r}^{s}$，记为$R$，保留未知量，得$\overset{\sim}R$:
$\overset{\sim}{R}=R-\rho _0+c\cdot \delta t_r-\delta _{\varSigma}\\\overset{\sim}{R}=\left[ \frac{-\left( x_s-x_{r}^{0} \right)}{\rho _0},\frac{-\left( y_s-y_{r}^{0} \right)}{\rho _0},\frac{-\left( z_s-z_{r}^{0} \right)}{\rho _0},1 \right] \left[ \begin{array}{c} \varDelta x_r\\ \varDelta y_r\\ \varDelta z_r\\ c\delta t_r\\\end{array} \right] +\varepsilon$
记为$L=AX+\upsilon$
多个$L=AX+\upsilon$进行联立，采用最小二乘法进行求解
注意点：

• $x=\left( x_{r}^{0},y_{r}^{0},z_{r}^{0} \right)$表示为用户接收机天线位置的近似值，通常初始化为$(0,0,0)$。我们将计算出来的$x=\left( x_{r}^{0},y_{r}^{0},z_{r}^{0} \right)$作为下一次的$(x_r,y_r,z_r)$，通过多次迭代计算后，就可以获取误差值最小的 $(x_r,y_r,z_r)$ ，也就是我们需要计算的接收机天线位置坐标。
  $x=dx+x_0=\left[ \begin{array}{c} \varDelta x\\ \varDelta y\\ \varDelta z\\\end{array} \right] +\left[ \begin{array}{c} x_0\\ y_0\\ z_0\\\end{array} \right]$

程序实现步骤：

1. 读取卫星星历数据、原始观测量数据（即卫星的伪距、载波等）；
2. 通过卫星星历以及伪距信息计算出卫星基于ECEF的坐标、每个卫星的钟差；
3. 将接收机天线位置初始化为(0,0,0)；
4. 根据推导的方程中各公式，结合卫星坐标、时间、伪距信息计算出对应的数值；
5. 根据最小二乘法，将上述的数据整理为对应的矩阵；
6. 根据矩阵计算法则计算出的值；
7. 重复4-6步，并设定值的变化满足一个很小的值的时候，结束迭代；
8. 输出对应的坐标。