观测模型

伪距观测方程

伪距观测值代表卫星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)+D_r+D^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))'\]
由于电离层误差，对流层误差和硬件延迟的时间导数很小可忽略不计，统一在测量误差项中，化简有多普勒观测方程

载波相位观测方程

载波相位观测值为卫星和接收机之间特定频率的载波相位之差，实际中，信号接收机只能测量接收机和卫星的载波相位差的小数部分，整数部分无法确定。
\[{\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)\)为卫星和接收的距离，则得到载波相位观测方程

伪距定位原理

根据伪距观测方程 \(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)\) 向量\(X=(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) = \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)=A(X)-b\) 进行求解 \(min||f(x)||\)
\[\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)=\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)\)后 在计算增量\(\Delta x\)

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

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

迭代方程，直到收敛为止

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

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

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

待补充