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

观测模型

伪距观测方程

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

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

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

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

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

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

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

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

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

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

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

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

(Delta t^s)：卫星钟差, (cDelta 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}'={ ho^s_r}'+{cDelta t_r}'-{cDelta t^s}'+(c[dcb_r+dcb^s]+I^s_r+T^s_r+dm^s_r+e^s_r(t))' ]

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

(D^s_r={ ho^s_r}'+{cDelta t_r}'-{cDelta t^s}'+{e^s_r(t)}')

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

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

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

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

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

载波相位观测方程

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

[{varphi}^{s}_r(t)={varphi}_r(t)-{varphi}^s(t- au^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- au^s_r)=freq*(t- au^s_r+Delta t^s)+{varphi}^s(0))

整理有

[{varphi}^{s}_r(t)=freq*( au^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( au^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)= ho^s_r+cDelta t_r-cDelta 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))：载波相位观测值

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

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

(Delta t^s)：卫星钟差, (cDelta 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= ho^s_r+cDelta t_r-cDelta t^s+c[Dcb_r+Dcb^s]+I^s_r+T^s_r+dm^s_r+e^s_r(t))

忽略大气层，多路径，硬件延迟和测量误差等因素，方程简化为(P^s_r= ho^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{egin{matrix} {P_1}^s_r \ {P_2}^s_r \ {P_3}^s_r \ vdots \ {P_N}^s_r end{matrix} ight} = left{egin{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} ight} = left{egin{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} ight} ]

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

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

伪距观测量方程组方程组 (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{egin{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} ight})

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

[egin{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{egin{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} ight} = left{egin{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} ight} ]

[J(x_r,y_r,z_r,Delta T_r)=left{egin{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} ight} ]

设(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{egin{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} ight} ]

得到(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{egin{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} ight})

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

多普勒定速原理

有多普勒观测方程 (D^s_r={ ho^s_r}'+{cDelta t_r}'-{cDelta 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))

其中

[{ ho^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}) 则

[{ ho^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+{cDelta t_r}'-{cDelta 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{egin{matrix} vx_r\ vy_r\ vz_r\ Delta T_r' end{matrix} ight} = 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{egin{matrix} vx_k\ vy_k\ vz_k end{matrix} ight}+{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{egin{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} ight} left{egin{matrix} vx_r\ vy_r\ vz_r\ Delta T_r' end{matrix} ight} = left{egin{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} ight} ]

[left{egin{matrix} I_1 \ I_2 \ vdots\ I_n end{matrix} ight} left{egin{matrix} vx_r\ vy_r\ vz_r\ Delta T_r' end{matrix} ight} = left{egin{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} ight} ]

这里方程左边的常量项和伪距定位方程中的几何矩阵(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{egin{matrix} I_1 \ I_2 \ vdots\ I_n end{matrix} ight} left{egin{matrix} vx_r\ vy_r\ vz_r\ Delta T_r' end{matrix} ight} - left{egin{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} ight} ]

[ f(X_r) = left{egin{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} ight} ]

(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}'))

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