GNSS学习笔记-观测量模型和定位定速方程

观测模型

伪距观测方程

伪距观测值代表卫星Satellite和接收机Receiver之间粗略的距离信息由\(P^s_{r,k}\),其中S代表卫星，r代表接收机，k代码第k颗卫星。它由用户接收到信号的时间\(t_r(t)\)和卫星发射信号的时间\(t^s(t-\tau^s_r)\)相减再乘以光速得到的，\(\tau^s_r\)为信号传播时间，公式为

\[P^s_r=c*[t_r(t)-t^s(t-\tau^s_r)]+e^s_r(t) \]

\(e^s_r(t)\)：伪距观测噪声

考虑到接收机时间误差 \(t_r(t)=t+\Delta t_r\)

考虑到卫星钟误差 \(t^s(t-\tau^s_r)=t-\tau^s_r+\Delta t^s\)

信号传播时间 \(\tau^s_r=\tau^s_{r,real}+dcb_r+dcb^s=\frac{1}{c}*(\rho^s_r+I^s_r+T^s_r+dm^s_r)+dcb_r+dcb^s\)

综合上述得到伪距观测方程

\(P^s_r=\rho^s_r+c\Delta t_r-c\Delta t^s+c[dcb_r+dcb^s]+I^s_r+T^s_r+dm^s_r+e^s_r(t)\)

########################################################################################

\(P^s_r\)：伪距观测值，已知变量，可通过接收机测量获得

\(\rho^s_r\)：卫地距，实际卫星和接收机的距离

\(\Delta t_r\)：接收机钟差，\(c\Delta t_r=\Delta T_r\)

\(\Delta t^s\)：卫星钟差, \(c\Delta t^s=\Delta T^s\)

\(dcb_r\)：接收机硬件延迟, \(c*dcb_r=Dcb_r\)

\(dcb^s\)：卫星硬件延迟，\(c*dcb^s=Dcb^s\)

\(I^s_r\)：电离层误差

\(T^s_r\)：对流层误差

\(dm^s_r\)：多路径误差

\(e^s_r(t)\)：伪距观测噪声

多普勒观测方程

对伪距观测方程进行时间求导有

\[{P^s_r}'={\rho^s_r}'+{c\Delta t_r}'-{c\Delta t^s}'+(c[dcb_r+dcb^s]+I^s_r+T^s_r+dm^s_r+e^s_r(t))' \]

由于电离层误差，对流层误差和硬件延迟的时间导数很小可忽略不计，统一在测量误差项中，化简有多普勒观测方程

\(D^s_r={\rho^s_r}'+{c\Delta t_r}'-{c\Delta t^s}'+{e^s_r(t)}'\)

########################################################################################

\(D^s_r={P^s_r}'\): 多普勒测量值，已知变量，可通过基带多普勒频移测量值获得

\({\Delta t_r}'\)：接收机钟漂,\({c\Delta t_r}'={\Delta T_r}'\)

\({\Delta t^s}'\)：卫星钟漂,\({c\Delta t^s}'={\Delta T^s}'\)

\({e^s_r(t)}'\)：多普勒观测误差

载波相位观测方程

载波相位观测值为卫星和接收机之间特定频率的载波相位之差，实际中，信号接收机只能测量接收机和卫星的载波相位差的小数部分，整数部分无法确定。

\[{\varphi}^{s}_r(t)={\varphi}_r(t)-{\varphi}^s(t-\tau^s_r)+N^s_r+e^s_r(t) \]

考虑到接收机初始相位\({\varphi}_r(0)\)，卫星初始相位\({\varphi}^s(0)\)和传播时间

接收机信号相位\({\varphi}_r(t)=freq*(t+\Delta t_r)+{\varphi}_r(0)\)

卫星信号相位\({\varphi}^s(t-\tau^s_r)=freq*(t-\tau^s_r+\Delta t^s)+{\varphi}^s(0)\)

整理有

\[{\varphi}^{s}_r(t)=freq*(\tau^s_r+\Delta t_r-\Delta t^s)+[{\varphi}_r(0)-{\varphi}^s(0)]+N^s_r+e^s_r(t) \]

又有波长\(\lambda=c/freq\),方程两边同时乘以\(\lambda\)

\[\lambda{\varphi}^{s}_r(t)=c(\tau^s_r+\Delta t_r-\Delta t^s)+\lambda[{\varphi}_r(0)-{\varphi}^s(0)]+\lambda N^s_r+\lambda e^s_r(t) \]

\(\lambda{\varphi}^{s}_r(t)=L^{s}_r(t)\)为卫星和接收的距离，则得到载波相位观测方程

\(L^{s}_r(t)=\rho^s_r+c\Delta t_r-c\Delta t^s+c[dcb_r+dcb^s]+I^s_r+T^s_r+dm^s_r+\lambda[{\varphi}_r(0)-{\varphi}^s(0)]+\lambda N^s_r+e^s_r(t)\)

########################################################################################

\(L^{s}_r(t)\)：载波相位观测值

\(\rho^s_r\)：卫地距，实际卫星和接收机的距离

\(\Delta t_r\)：接收机钟差，\(c\Delta t_r=\Delta T_r\)

\(\Delta t^s\)：卫星钟差, \(c\Delta t^s=\Delta T^s\)

\(dcb_r\)：接收机硬件延迟, \(c*dcb_r=Dcb_r\)

\(dcb^s\)：卫星硬件延迟，\(c*dcb^s=Dcb^s\)

\(I^s_r\)：电离层误差

\(T^s_r\)：对流层误差

\(dm^s_r\)：多路径误差

\({\varphi}_r(0)\)：接收机初始相位

\({\varphi}^s(0)\)：卫星初始相位

\(N^s_r\)：整周模糊度

\(e^s_r(t)\)：载波相位观测误差

伪距定位原理

根据伪距观测方程 \(P^s_r=\rho^s_r+c\Delta t_r-c\Delta t^s+c[Dcb_r+Dcb^s]+I^s_r+T^s_r+dm^s_r+e^s_r(t)\)

忽略大气层，多路径，硬件延迟和测量误差等因素，方程简化为\(P^s_r=\rho^s_r+\Delta T_r\)

假设卫星k位置为\((x_k,y_k,z_k)\),接收机位置为\((x_r,y_r,z_r)\),则有

\[{P_k}^s_r=\sqrt{(x_k-x_r)^2+(y_k-y_r)^2+(z_k-z_r)^2}+\Delta T_r=f_k(x_r,y_r,z_r,\Delta T_r) \]

当有N颗卫星时，则有伪距观测量方程组

\[\left\{\begin{matrix} {P_1}^s_r \\ {P_2}^s_r \\ {P_3}^s_r \\ \vdots \\ {P_N}^s_r \end{matrix} \right\} = \left\{\begin{matrix} \sqrt{(x_1-x_r)^2+(y_1-y_r)^2+(z_1-z_r)^2}+\Delta T_r \\ \sqrt{(x_2-x_r)^2+(y_2-y_r)^2+(z_2-z_r)^2}+\Delta T_r \\ \sqrt{(x_3-x_r)^2+(y_3-y_r)^2+(z_3-z_r)^2}+\Delta T_r \\ \vdots \\ \sqrt{(x_N-x_r)^2+(y_N-y_r)^2+(z_N-z_r)^2}+\Delta T_r \end{matrix} \right\} = \left\{\begin{matrix} f_1(x_r,y_r,z_r,\Delta T_r)\\ f_2(x_r,y_r,z_r,\Delta T_r)\\ f_3(x_r,y_r,z_r,\Delta T_r)\\ \vdots \\ f_N(x_r,y_r,z_r,\Delta T_r) \end{matrix} \right\} \]

该非线性方程可用最小二乘进行求解

最小二乘求解伪距定位方程

伪距观测量方程组方程组 \(b=A(X_r)\) 向量\(X_r=(x_r,y_r,z_r,\Delta T_r)\) 接收机坐标和接收机钟差

\(b=({P_1}^s_r, {P_2}^s_r, {P_3}^s_r, \cdots {P_N}^s_r)^T\)

\(A(X_r) = \left\{\begin{matrix} \sqrt{(x_1-x_r)^2+(y_1-y_r)^2+(z_1-z_r)^2}+\Delta T_r \\ \sqrt{(x_2-x_r)^2+(y_2-y_r)^2+(z_2-z_r)^2}+\Delta T_r \\ \sqrt{(x_3-x_r)^2+(y_3-y_r)^2+(z_3-z_r)^2}+\Delta T_r \\ \vdots \\ \sqrt{(x_N-x_r)^2+(y_N-y_r)^2+(z_N-z_r)^2}+\Delta T_r \end{matrix} \right\}\)

根据如下非线性最小二乘方法求解方法，\(f(X_r)=A(X_r)-b\) 进行求解 \(min||f(X_r)||\)

\[\begin{cases} X_{k+1}=X_k+\Delta X \\ \Delta X=[J(X)^TJ(X)^{-1}]*J(X)^T*f(X) \end{cases}\]

其中 \(J(X)\)为Jacobian矩阵，当有n颗卫星时表示为

\[J(X_r)=\left\{\begin{matrix} \frac{\partial f_1(x_1)}{\partial x} & \frac{\partial f_1(x_2)}{\partial x} & \cdots & \frac{\partial f_1(x_m)}{\partial x} \\ \frac{\partial f_2(x_1)}{\partial x} & \frac{\partial f_2(x_2)}{\partial x} & \cdots & \frac{\partial f_2(x_m)}{\partial x} \\ \vdots\\ \frac{\partial f_n(x_1)}{\partial x} & \frac{\partial f_n(x_2)}{\partial x} & \cdots & \frac{\partial f_n(x_m)}{\partial x} \\ \end{matrix} \right\} = \left\{\begin{matrix} \frac{\partial f_1(x_r,y_r,z_r,\Delta T_r)}{\partial x_r} & \frac{\partial f_1(x_r,y_r,z_r,\Delta T_r)}{\partial y_r} & \frac{\partial f_1(x_r,y_r,z_r,\Delta T_r)}{\partial z_r} & \frac{\partial f_1(x_r,y_r,z_r,\Delta T_r)}{\Delta T_r} \\ \frac{\partial f_2(x_r,y_r,z_r,\Delta T_r)}{\partial x_r} & \frac{\partial f_2(x_r,y_r,z_r,\Delta T_r)}{\partial y_r} & \frac{\partial f_2(x_r,y_r,z_r,\Delta T_r)}{\partial z_r} & \frac{\partial f_2(x_r,y_r,z_r,\Delta T_r)}{\Delta T_r} \\ \vdots\\ \frac{\partial f_n(x_r,y_r,z_r,\Delta T_r)}{\partial x_r} & \frac{\partial f_n(x_r,y_r,z_r,\Delta T_r)}{\partial y_r} & \frac{\partial f_n(x_r,y_r,z_r,\Delta T_r)}{\partial z_r} & \frac{\partial f_n(x_r,y_r,z_r,\Delta T_r)}{\Delta T_r} \\ \end{matrix} \right\} \]

\[J(x_r,y_r,z_r,\Delta T_r)=\left\{\begin{matrix} \frac{-(x_1-x_r)}{\sqrt{(x_1-x_r)^2+(y_1-y_r)^2+(z_1-z_r)^2}} & \frac{-(y_1-y_r)}{\sqrt{(x_1-x_r)^2+(y_1-y_r)^2+(z_1-z_r)^2}} & \frac{-(z_1-z_r)}{\sqrt{(x_1-x_r)^2+(y_1-y_r)^2+(z_1-z_r)^2}} & 1 \\ \frac{-(x_2-x_r)}{\sqrt{(x_2-x_r)^2+(y_2-y_r)^2+(z_2-z_r)^2}} & \frac{-(y_2-y_r)}{\sqrt{(x_2-x_r)^2+(y_2-y_r)^2+(z_2-z_r)^2}} & \frac{-(z_2-z_r)}{\sqrt{(x_2-x_r)^2+(y_2-y_r)^2+(z_2-z_r)^2}} & 1 \\ \vdots\\ \frac{-(x_n-x_r)}{\sqrt{(x_n-x_r)^2+(y_1-y_r)^2+(z_n-z_r)^2}} & \frac{-(y_n-y_r)}{\sqrt{(x_n-x_r)^2+(y_n-y_r)^2+(z_n-z_r)^2}} & \frac{-(z_n-z_r)}{\sqrt{(x_n-x_r)^2+(y_n-y_r)^2+(z_n-z_r)^2}} & 1 \end{matrix} \right\} \]

设\(R_n=\sqrt{(x_n-x_r)^2+(y_n-y_r)^2+(z_n-z_r)^2}\) 则

\[J(x_r,y_r,z_r,\Delta T_r)= \left\{\begin{matrix} \frac{(x_r-x_1)}{R_1} & \frac{(y_r-y_1)}{R_1} & \frac{(z_r-z_1)}{R_1} & 1 \\ \frac{(x_r-x_2)}{R_2} & \frac{(y_r-y_2)}{R_2} & \frac{(z_r-z_2)}{R_2} & 1 \\ \vdots\\ \frac{(x_r-x_n)}{R_n} & \frac{(y_r-y_n)}{R_n} & \frac{(z_r-z_n)}{R_n} & 1 \end{matrix} \right\} \]

得到\(J(X_r)\)后 在计算增量\(\Delta x\)

\(\Delta X=[J(X_r)^TJ(X_r))^{-1}]*J(X_r))^T*f(X_r)\)

\(\Delta X=[J(X_r)^TJ(X_r))^{-1}]*J(X_r))^T*[A(X_r)-b]\)

值得指出的是 这里\(J(X_r)\)只和卫星和接收机位置相关，称为几何矩阵，\(A(X_r)-b\)为测量伪距和计算的卫地距之差，称为伪距残差

迭代方程，直到收敛为止

\(\Delta X=[J(X_r)^TJ(X_r)^{-1}]*J(X_r)^T \left\{\begin{matrix} \sqrt{(x_1-x_r)^2+(y_1-y_r)^2+(z_1-z_r)^2}+\Delta T_r-{P_1}^s_r\\ \sqrt{(x_2-x_r)^2+(y_2-y_r)^2+(z_2-z_r)^2}+\Delta T_r-{P_2}^s_r\\ \vdots \\ \sqrt{(x_n-x_r)^2+(y_n-y_r)^2+(z_n-z_r)^2}+\Delta T_r-{P_n}^s_r \end{matrix} \right\}\)

\(X_{k+1}=X_r+\Delta X\)

多普勒定速原理

有多普勒观测方程 \(D^s_r={\rho^s_r}'+{c\Delta t_r}'-{c\Delta t^s}'+{e^s_r(t)}'\)

假设卫星k位置为\((x_k,y_k,z_k)\),接收机位置为\((x_r,y_r,z_r)\)

假设卫星k速度为\((\frac{\partial x_k}{\partial t},\frac{\partial y_k}{\partial t},\frac{\partial z_k}{\partial t})=(vx_k,vy_k,vz_k)\),接收机速度为\((\frac{\partial x_r}{\partial t},\frac{\partial y_r}{\partial t},\frac{\partial z_r}{\partial t})=(vx_r,vy_r,vz_r)\)

其中

\[{\rho^s_r}'=\frac{\partial \sqrt{(x_k-x_r)^2+(y_k-y_r)^2+(z_k-z_r)^2}}{\partial t}\\ =\frac{x_k-x_r}{\sqrt{(x_k-x_r)^2+(y_k-y_r)^2+(z_k-z_r)^2}}*(\frac{\partial x_k}{\partial t} - \frac{\partial x_r}{\partial t}) + \\ \frac{y_k-y_r}{\sqrt{(x_k-x_r)^2+(y_k-y_r)^2+(z_k-z_r)^2}}*(\frac{\partial y_k}{\partial t} - \frac{\partial y_r}{\partial t}) + \\ \frac{z_k-z_r}{\sqrt{(x_k-x_r)^2+(y_k-y_r)^2+(z_k-z_r)^2}}*(\frac{\partial z_k}{\partial t} - \frac{\partial z_r}{\partial t}) \]

设\(R_n=\sqrt{(x_n-x_r)^2+(y_n-y_r)^2+(z_n-z_r)^2}\) 则

\[{\rho^s_r}'=\frac{x_k-x_r}{R_k}*vx_k+\frac{y_k-y_r}{R_k}*vy_k+\frac{z_k-z_r}{R_k}*vz_k-\frac{x_k-x_r}{R_k}*vx_r-\frac{y_k-y_r}{R_k}*vy_r-\frac{z_k-z_r}{R_k}*vz_r \]

并代入多普勒观测方程为

\[D^s_r=\frac{x_k-x_r}{R_k}*vx_k+\frac{y_k-y_r}{R_k}*vy_k+\frac{z_k-z_r}{R_k}*vz_k-\frac{x_k-x_r}{R_k}*vx_r-\frac{y_k-y_r}{R_k}*vy_r-\frac{z_k-z_r}{R_k}*vz_r+{c\Delta t_r}'-{c\Delta t^s}'+{e^s_r(t)}' \]

将接收机速度项放到左边，多普勒测量值放到右边，

\[(\frac{x_r-x_k}{R_k},\frac{y_r-y_k}{R_k},\frac{z_r-z_k}{R_k},1)*\left\{\begin{matrix} vx_r\\ vy_r\\ vz_r\\ \Delta T_r' \end{matrix} \right\} = D^s_r+(\frac{x_r-x_k}{R_k},\frac{y_r-y_k}{R_k},\frac{z_r-z_k}{R_k}) *\left\{\begin{matrix} vx_k\\ vy_k\\ vz_k \end{matrix} \right\}+{\Delta T^s}'-{e^s_r(t)}'\]

上述右边公式中，卫星速度和卫星钟差可通过星历计算获得。

设
\((\frac{x_r-x_k}{R_k},\frac{y_r-y_k}{R_k},\frac{z_r-z_k}{R_k},1)=I_k\)

当有n颗卫星进行定位时，误差项\({e^s_r(t)}'\)不略不计，有方程组

\[\left\{\begin{matrix} \frac{(x_r-x_1)}{R_1} & \frac{(y_r-y_1)}{R_1} & \frac{(z_r-z_1)}{R_1} & 1 \\ \frac{(x_r-x_2)}{R_2} & \frac{(y_r-y_2)}{R_2} & \frac{(z_r-z_2)}{R_2} & 1 \\ \vdots\\ \frac{(x_r-x_n)}{R_n} & \frac{(y_r-y_n)}{R_n} & \frac{(z_r-z_n)}{R_n} & 1 \end{matrix} \right\} \left\{\begin{matrix} vx_r\\ vy_r\\ vz_r\\ \Delta T_r' \end{matrix} \right\} = \left\{\begin{matrix} {D_1}^s_r+(\frac{x_r-x_1}{R_1},\frac{y_r-y_1}{R_1},\frac{z_r-z_1}{R_1},1)*(vx_1,vy_1,vz_1,{\Delta T^s}')^T\\ {D_2}^s_r+(\frac{x_r-x_2}{R_2},\frac{y_r-y_2}{R_2},\frac{z_r-z_2}{R_2},1)*(vx_2,vy_2,vz_2,{\Delta T^s}')^T\\ \vdots\\ {D_n}^s_r+(\frac{x_r-x_n}{R_n},\frac{y_r-y_n}{R_n},\frac{z_r-z_n}{R_n},1)*(vx_n,vy_n,vz_n,{\Delta T^s}')^T\\ \end{matrix} \right\} \]

\[\left\{\begin{matrix} I_1 \\ I_2 \\ \vdots\\ I_n \end{matrix} \right\} \left\{\begin{matrix} vx_r\\ vy_r\\ vz_r\\ \Delta T_r' \end{matrix} \right\} = \left\{\begin{matrix} {D_1}^s_r+I_1*(vx_1,vy_1,vz_1,{\Delta T^s}')^T\\ {D_2}^s_r+I_2*(vx_2,vy_2,vz_2,{\Delta T^s}')^T\\ \vdots\\ {D_n}^s_r+I_n*(vx_n,vy_n,vz_n,{\Delta T^s}')^T\\ \end{matrix} \right\} \]

这里方程左边的常量项和伪距定位方程中的几何矩阵\(J(X_r)\)完全相同，可用最小二乘求解上述方程

最小二乘求解多普勒定速方程

将上述的多普勒定速方程组简写为

\[AX_r=B \]

采用最小二乘求解\(X_r=(vx_r,vy_r,vz_r,{\Delta T_r}')\)，\(min{||f(X_r)||}^2=min{||A*X_r-B||}^2\)

\[f(X_r)=A*X_r-B= \left\{\begin{matrix} I_1 \\ I_2 \\ \vdots\\ I_n \end{matrix} \right\} \left\{\begin{matrix} vx_r\\ vy_r\\ vz_r\\ \Delta T_r' \end{matrix} \right\} - \left\{\begin{matrix} {D_1}^s_r+I_1*(vx_1,vy_1,vz_1,{\Delta T^s}')^T\\ {D_2}^s_r+I_2*(vx_2,vy_2,vz_2,{\Delta T^s}')^T\\ \vdots\\ {D_n}^s_r+I_n*(vx_n,vy_n,vz_n,{\Delta T^s}')^T\\ \end{matrix} \right\} \]

\[ f(X_r) = \left\{\begin{matrix} I_1*(vx_r-vx_1,vy_r-vy_1,vz_r-vz_1,\Delta T_r'-{\Delta T^s}')^T - {D_1}^s_r\\ I_2*(vx_r-vx_2,vy_r-vy_2,vz_r-vz_2,\Delta T_r'-{\Delta T^s}')^T - {D_2}^s_r\\ \vdots\\ I_n*(vx_r-vx_n,vy_r-vy_n,vz_r-vz_n,\Delta T_r'-{\Delta T^s}')^T - {D_n}^s_r\\ \end{matrix} \right\} \]

\(f(X_r)\)为多普勒残差, 求导得雅克比矩阵，\(J(X_r)=\frac{\partial(A*X_r-B)}{\partial X_r}=A\)
迭代如下公式直到收敛，即可求出\((vx_r,vy_r,vz_r,{\Delta T_r}')\)

\[\begin{cases} {X_r}_{k+1}={X_r}_{k}+\Delta X_r \\ \Delta X_r=[A^TA^{-1}]*A^T*f(X_r) \end{cases}\]