观测模型
伪距观测方程
伪距观测值代表卫星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}]