首页 - 技术栈

创建建站app外包公司怎么找

作者: 五速梦信息网
时间: 2026年03月21日 11:30

当前位置：首页 > news >正文

创建建站,app外包公司怎么找,wordpress在哪里下载,建简单网站原始 Markdown文档、Visio流程图、XMind思维导图见#xff1a;https://github.com/LiZhengXiao99/Navigation-Learning 文章目录一、SPP 解算1、spp()#xff1a;单点定位主入口函数2、estpos()3、estpose()4、valsol()#xff1a;GDOP和卡方检验结果有效性二、卫星位置钟… 原始 Markdown文档、Visio流程图、XMind思维导图见https://github.com/LiZhengXiao99/Navigation-Learning 文章目录一、SPP 解算1、spp()单点定位主入口函数2、estpos()3、estpose()4、valsol()GDOP和卡方检验结果有效性二、卫星位置钟差计算1、satpossrtklib()2、ephclk()1. eph2clk()时钟校正参数 a f 0 、 a f 1 、 a f 2 a{f0}、a{f1}、a{f2} af0、af1、af2计算卫星钟差相对论效应校正 Δ t r \Delta tr Δtr群波延迟校正 T G D T{GD} TGD钟漂校正 2. geph2clk()时钟校正参数 τ n 、 γ n \tau{n}、\gamma{n} τn、γn计算 GLONASS 卫星钟差 3、ephpos()1. eph2pos()2. var_uraeph()用URA用户测距精度标定卫星位置方差3. geph2pos()由 GLONASS 星历计算卫星位置钟差4. glorbit()龙格库塔迭代5. deq()微分方程计算 4、peph2pos()精密星历计算卫星位置、钟差、速度、钟漂1. 精密星历读取流程2. pephpos()精密星历计算卫星位置钟差3. interppol()Neville 插值4. posWithEarhRotation()5. pephclk()精密钟差计算卫星钟差三、rescode()残差计算、设计矩阵构建1、geodist()计算近似几何距离、接收机到卫星方向的观测矢量sagnac 效应改正2、satazel()计算方位角高度角3、prange()差分码偏差改正4、gettgd()4、satexclude()排除不可用卫星的观测值5、ionocorr()根据选项模型计算 L1 电离层延迟 I6、ionmodel()克罗布歇模型电离层延迟计算7、tropcorr()对流层改正8、tropmodel()Saastamoinen 模型计算对流层延迟7、varerr()计算伪距量测协方差8、getHVR_spp 四、lsqplus()最小二乘估计一、SPP 解算 1、spp()单点定位主入口函数默认使用广播星历计算卫星位置、钟差使用克罗布歇模型通过广播星历中的参数计算电离层延迟使用 Saastamoinen 模型计算对流层延迟。调用 satpossrtklib() 计算卫星位置、卫星钟差 rs[(0:2)i*6]卫星位置 {x,y,z} (m)rs [(3:5)i*6]卫星速度 {vx,vy,vz} (m/s)dts[(0:1)i*2]卫星钟差 {bias,drift} (s|s/s)var[i] 卫星位置和钟差的协方差 (m^2)svh[i]卫星健康标志 (-1:correction not available) 调用 estpos() 计算接收机位置加权最小二乘其中会调用 valsol 进行卡方检验和GDOP检验。存入方位角和俯仰角赋值解算状态结构体 ssat。
2、estpos() 3、estpose() 先给矩阵开辟内存空间待估参数 dx 赋值为 0然后进入最小二乘迭代求解先调用 rescode() 计算当前迭代的伪距残差 v、设计矩阵 H、伪距残差的方差 var、所有观测卫星的方位角和仰角 azel定位时有效性 vsat、定位后伪距残差 resp、参与定位的卫星个数 ns和方程个数 nv。然后对 H、V 协方差加权以 varerr() 计算出的伪距残差的标准差的倒数作为权重对H和v分别左乘权重对角阵得到加权之后的 H 和 v 。 W diag ⁡ ( σ 1 − 2 , σ 2 − 2 , … , σ m − 2 ) σ 2 F s R r ( a σ 2 b σ 2 / sin ⁡ E l r s ) σ eph 2 σ ion 2 σ trop 2 σ bias 2 \begin{array}{l}\boldsymbol{W}\operatorname{diag}\left(\sigma{1}^{-2}, \sigma{2}^{-2}, \ldots, \sigma{m}^{-2}\right) \ \sigma^{2}F^{s} R{r}\left(a{\sigma}{ }^{2}b{\sigma}{ }^{2} / \sin E l{r}^{s}\right){\sigma{\text {eph }}}^{2}{\sigma{\text {ion }}}^{2}{\sigma{\text {trop }}}^{2}{\sigma{\text {bias }}}^{2}\end{array} Wdiag(σ1−2,σ2−2,…,σm−2)σ2FsRr(aσ2bσ2/sinElrs)σeph 2σion 2σtrop 2σbias 2 调用 lsqPlus() 最小二乘解算计算状态 x 的增量 dx 最后状态反馈把 dx 置加到 x 上
结束迭代有两种情况最小二乘求解后dx 的模 d0 小于 1E-4 存结果到 sol调用 valsol() 卡方和 GDOP 检验结果有效结果状态设为单点解。超出迭代次数上限10次不存结果输出迭代发散的消息。
static int estpos(int *bDeleted, double *x,const obsd_t *obs, int n, const double *rs, const double *dts, const double *vare, const int *svh, const nav_t *nav, const prcopt_t *opt, sol_t *sol, double *azel, int *vsat, double *resp, char *msg) {int i,j,k,info,stat0,nx,nv,bElevCVG;double dx[NX_SPP],Q[NX_SPP*NX_SPP],dop[4],*v,*H,var,sig,d0;bElevCVG0;vmat(n5,1); Hmat(NX_SPP,n5); varmat(n5,1);for (i0;iNX_SPP;i) dx[i]0.0;for (i0;iMAXITR;i) {// 调用 rescode() 计算设计矩阵 H、残差 V/ pseudorange residuals /nvrescode(i,bElevCVG,obs,n,rs,dts,vare,svh,nav,x,opt,v,H,var,azel,vsat,resp,nx,bDeleted);if (nvnx) {sprintf(msg,lack of valid sats ns%d,nv);break;}// H、V 协方差加权/ weight by variance */for (j0;jnv;j) {sigsqrt(var[j]);v[j]/sig;for (k0;kNX_SPP;k) H[kjNX_SPP]/sig;}// 调用 lsqPlus() 最小二乘解算/ least square estimation /if ((infolsqPlus(H,v,NX_SPP,nv,dx,Q))) {sprintf(msg,lsq error info%d,info);sprintf(PPP_Glo.chMsg, %s\n,msg);outDebug(OUTWIN,OUTFIL,OUTTIM);break;}// 状态反馈for (j0;jNX_SPP;j) x[j]dx[j];d0norm(dx,NX_SPP);if (d01E4) bElevCVG1;else bElevCVG0;if (d01E-4) {sol-type0;sol-timetimeadd(obs[0].time,-x[3]/CLIGHT); // 加上钟差sol-dtr[0]x[3]/CLIGHT; / receiver clock bias (s) /sol-dtr[1]x[4]/CLIGHT; / glo-gps time offset (s) /sol-dtr[2]x[5]/CLIGHT; / bds-gps time offset (s) /sol-dtr[3]x[6]/CLIGHT; / gal-gps time offset (s) /sol-dtr[4]x[7]/CLIGHT; / qzs-gps time offset (s) */for (j0;j6;j) sol-rr[j]j3?x[j]:0.0;for (j0;j3;j) sol-qrjQ[jjNX_SPP];sol-qr3Q[1]; / cov xy /sol-qr4Q[2NX_SPP]; / cov yz /sol-qr5Q[2]; / cov zx /sol-ns0nv;sol-rmssol-dop[0]sol-dop[1]sol-dop[2]sol-dop[3]0.0;// 调用 valsol() 卡方和 GDOP 检验结果有效性/ validate solution */if ((statvalsol(azel,vsat,n,opt,v,nv,nx,msg,dop)))sol-statSOLQ_SINGLE;sol-dop[0]dop[0];sol-dop[1]dop[1];sol-dop[2]dop[2];sol-dop[3]dop[3];break;}}if (iMAXITR) sprintf(msg,iteration divergent i%d,i);free(v); free(H); free(var);return stat; }4、valsol()GDOP和卡方检验结果有效性低成本接收机可能通不过检验可禁用此函数 const double *azel 方位角、高度角 const int *vsat 观测卫星在当前定位时是否有效 (1*n) int n 观测值个数 const prcoptt *opt 处理选项 const double *v 定位方程的右端部分伪距残差 int nv 观测值数 int nx 待估计参数数 char *msg 错误消息v s ( P r s − ( ρ ^ r s c d ^ t r − c d T s I r s T r s ) ) σ s v ( v 1 , v 2 , v 3 , … , v m ) T v T v m − n − 1 χ α 2 ( m − n − 1 ) G D O P G D O P thres \begin{array}{l}v{s}\frac{\left(P{r}^{s}-\left(\hat{\rho}{r}^{s}c \hat{d} t{r}-c d T^{s}I{r}^{s}T{r}^{s}\right)\right)}{\sigma{s}} \ \boldsymbol{v}\left(v{1}, v{2}, v{3}, \ldots, v{m}\right)^{T} \ \frac{\boldsymbol{v}^{T} \boldsymbol{v}}{m-n-1}\chi{\alpha}^{2}(m-n-1) \ G D O PG D O P{\text {thres }}\end{array} vsσs(Prs−(ρ^rscd^tr−cdTsIrsTrs))v(v1,v2,v3,…,vm)Tm−n−1vTvχα2(m−n−1)GDOPGDOPthres 二、卫星位置钟差计算 1、satposs_rtklib() gtime_t teph I (gpst) 用于选择星历的时刻 (gpst) obsd_t *obs I OBS观测数据 int n I OBS数 navt *nav I NAV导航电文 int ephopt I 星历选项 (EPHOPT???) double *rs O 卫星位置和速度长度为6*n{x,y,z,vx,vy,vz}(ecef)(m,m/s) double *dts O 卫星钟差长度为2*n {bias,drift} (s|s/s) double *var O 卫星位置和钟差的协方差 (m^2) int *svh O 卫星健康标志 (-1:correction not available)遍历观测数据找伪距观测值除以光速得到信号传播时间用数据接收时刻减去伪距信号传播时间得到信号发射时刻。调用 ephclk() 函数由广播星历计算出当前观测卫星与 GPS 时间的钟差 dt ,此时的钟差是没有考虑相对论效应和 TGD 的 dt 仅作为satpos()的参数不作为最终计算的钟差。信号发射时刻减去钟差 dt得到 GPS 时间下的卫星信号发射时刻。调用 satpos() 对此观测值进行下一步卫星位置钟差的计算satpos() 函数对星历计算选项进行判断广播星历模式调用 ephpos()精密星历模式调用 peph2pos()。最后检测钟差值如果没有精密星历则调用 ephclk() 用广播星历计算钟差。
extern void satposs_rtklib(gtime_t teph, const obsd_t *obs, int n, const nav_t *nav,int ephopt, double *rs, double *dts, double *var, int *svh) {gtime_t time[MAXOBS]{{0}};double dt,pr;int i,j;for (i0;ini2*MAXOBS;i) {for (j0;j6;j) rs [ji*6]0.0;for (j0;j2;j) dts[ji2]0.0;var[i]0.0; svh[i]0;/ search any psuedorange /for (j0,pr0.0;jNFREQ;j) if ((probs[i].P[j])!0.0) break;if (jNFREQ) {sprintf(PPP_Glo.chMsg,** WARNING: no pseudorange %s sat%2d\n,time_str(obs[i].time,3),obs[i].sat);outDebug(OUTWIN,OUTFIL,0);continue;}/* transmission time by satellite clock /time[i]timeadd(obs[i].time,-pr/CLIGHT);/ satellite clock bias by broadcast ephemeris /if (!ephclk(time[i],teph,obs[i].sat,nav,dt)) {sprintf(PPP_Glo.chMsg,** WARNING: no broadcast clock %s sat%2d\n,time_str(time[i],3),obs[i].sat);outDebug(0,OUTFIL,0);continue;}time[i]timeadd(time[i],-dt);/* satellite position and clock at transmission time */if (!satpos(time[i],teph,obs[i].sat,ephopt,nav,rsi*6,dtsi2,vari,svhi)) {sprintf(PPP_Glo.chMsg,** WARNING: no ephemeris %s sat%2d\n,time_str(time[i],3),obs[i].sat);outDebug(0,0,0);continue;}/* if no precise clock available, use broadcast clock instead */if (dts[i*2]0.0) {if (!ephclk(time[i],teph,obs[i].sat,nav,dtsi*2)) continue;dts[1i*2]0.0;*varSQR(STD_BRDCCLK);}} }2、ephclk() 单观测值卫星钟差计算。由于 GLONASS 系统的计算和其它的区别较大先进行判断。如果不是 GLONASS 则调用 seleph() 选择与观测值对应的星历调用 eph2clk() 根据广播星历参数 a 0 a_0 a0、 a 1 a_1 a1、 a 2 a_2 a2 计算卫星钟差迭代 3 次如果是 GLONASS 则调用 selgeph() 选择与观测值对应的星历调用 geph2clk() 根据广播星历参数 t a u n t_aun taun、 g a u n g_aun gaun 计算卫星钟差迭代 3 次。
static int ephclk(gtime_t time, gtime_t teph, int sat, const nav_t *nav,double *dts) {eph_t *eph;geph_t *geph;int sys;syssatsys(sat,NULL);if (sysSYS_GPS||sysSYS_GAL||sysSYS_QZS||sysSYS_CMP) {if (!(ephseleph(teph,sat,-1,nav))) return 0;*dtseph2clk(time,eph);}else if (sysSYSGLO) {if (!(gephselgeph(teph,sat,-1,nav))) return 0;*dtsgeph2clk(time,geph);}else return 0;return 1; }1. eph2clk()时钟校正参数 a f 0 、 a f 1 、 a f 2 a{f0}、a{f1}、a{f2} af0、af1、af2计算卫星钟差相对于 GPS 时间卫星上作为时间和频率信号来源的原子钟也存在时间偏差和频率漂移。为确保各颗卫星的时钟与GPS时间同步GPS地面监控部分通过对卫星信号进行检测将卫星时钟在GPS时间t的卫星钟差 Δ t ( s ) \Delta t^{(s)} Δt(s) 描述为如下二项式 Δ t ( s ) a f 0 a f 1 ( t − t o c ) a f 2 ( t − t o c ) 2 \Delta t^{(s)}a{f0}a{f1}(t-t{oc})a{f2}(t-t_{oc})^2 Δt(s)af0af1(t−toc)af2(t−toc)2 extern double eph2clk(gtime_t time, const eph_t *eph) {double t;int i;ttimediff(time,eph-toc); // 计算与星历参考时间的偏差 dt t-toc// 利用二项式校正计算出卫星钟差从 dt中减去这部分然后再进行一次上述操作得到最终的 dtfor (i0;i2;i) {t-eph-f0eph-f1*teph-f2*t*t;}// 使用二项式校正得到最终的钟差return eph-f0eph-f1*teph-f2*t*t; }除此以外卫星钟差一般还需考虑相对论效应校正、群波延迟校正、钟漂校正相对论效应校正 Δ t r \Delta t_r Δtr 综合狭义相对论和广义相对论在高空中高速运行的卫星原子钟比地面上一模一样的原子钟每天要快 38000ns 每秒快 0.44ns 。如果不考虑相对论效应GPS 发上天两分钟内卫星原子钟就会失去定位作用。在地面上设计原子钟时可以减小一点点它的频率上天以后其时钟频率在地面上看来正好等于设计值。同时因为GPS运行轨道是椭圆而不是圆地面上计算机还有根据卫星当前位置做相对论效应的校如下 Δ t r F e s a s sin ⁡ E k \Delta t_rFe_s\sqrt{a_s} \sin Ek ΔtrFesas sinEk 群波延迟校正 T G D T{GD} TGD 由第一数据块给出只适用于单频。这样对于 L1 单频接收机卫星时钟总钟差值如下 δ t ( s ) Δ t ( s ) Δ t r − T G D \delta t^{(s)}\Delta t^{(s)}\Delta t{r}-T{G D} δt(s)Δt(s)Δtr−TGD 钟漂校正对上面卫星时钟总钟差值求导得 δ f ( s ) a f 1 2 a f 2 ( t − t o c ) Δ t ˙ r \delta f^{(s)}a{f 1}2 a{f 2}\left(t-t_{o c}\right)\Delta \dot{t}r δf(s)af12af2(t−toc)Δt˙r 群波延迟校正 T G D T{GD} TGD 的导数为 0相对论效应校正 Δ t r \Delta t_r Δtr 如下 Δ t ˙ r F e s a s E ˙ k cos ⁡ E k \Delta \dot{t}_rF e_s \sqrt{a_s} \dot{E}_k \cos E_k Δt˙rFesas E˙kcosEk

geph2clk()时钟校正参数 τ n 、 γ n \tau{n}、\gamma{n} τn、γn计算 GLONASS 卫星钟差 d T s ( t ) − τ n γ n ( t − t b ) d T^{s}(t)-\tau{n}\gamma{n}\left(t-t_{b}\right) dTs(t)−τnγn(t−tb) extern double geph2clk(gtime_t time, const gepht *geph) {double t;int i;ttimediff(time,geph-toe);for (i0;i2;i) {t–geph-taungeph-gamn*t;}return -geph-taungeph-gamn*t; }用 τ n 、 γ n \tau{n}、\gamma_{n} τn、γn计算 GLONASS 卫星钟差的时候已经考虑了相对论效应了无需再改正。 3、ephpos() 与 ephclk() 同理由于 GLONASS 系统的计算和其它的区别较大先进行判断。如果不是 GLONASS 则调用 seleph() 选择与观测值对应的星历调用 eph2pos() 根据广播星历中的开普勒轨道参数和摄动改正计算卫星位置对北斗 MEO、IGSO 卫星会进行特殊处理、校正卫星钟差的相对论效应、调用 var_uraeph() 用 URA 值来标定方差。如果是 GLONASS 则调用 selgeph() 选择与观测值对应的星历调用 geph2pos() 根据广播星历中 PZ-90 坐标系下卫星状态向量四阶龙格库塔迭代计算卫星位置。计算完一次位置之后加上一个极小的时间再计算一次位置两次计算出的时间作差求得卫星速度钟漂。
static int ephpos(gtime_t time, gtime_t teph, int sat, const nav_t *nav,int iode, double *rs, double *dts, double *var, int *svh) {eph_t *eph;geph_t *geph;seph_t *seph;double rst[3],dtst[1],tt1E-3;int i,sys;trace(4,ephpos : time%s sat%2d iode%d\n,time_str(time,3),sat,iode);syssatsys(sat,NULL); //调用 satsys 函数确定该卫星所属的导航系统。*svh-1;if (sysSYS_GPS||sysSYS_GAL||sysSYS_QZS||sysSYS_CMP||sysSYS_IRN) {if (!(ephseleph(teph,sat,iode,nav))) return 0; //调用 seleph 函数来选择广播星历。eph2pos(time,eph,rs,dts,var); //根据选中的广播星历调用 eph2pos 函数来计算信号发射时刻卫星的位置、钟差和相应结果的误差。timetimeadd(time,tt);eph2pos(time,eph,rst,dtst,var);*svheph-svh;}else if (sysSYS_GLO) {if (!(gephselgeph(teph,sat,iode,nav))) return 0;geph2pos(time,geph,rs,dts,var);timetimeadd(time,tt);geph2pos(time,geph,rst,dtst,var);*svhgeph-svh;}else if (sysSYS_SBS) {if (!(sephselseph(teph,sat,nav))) return 0;seph2pos(time,seph,rs,dts,var);timetimeadd(time,tt);seph2pos(time,seph,rst,dtst,var);svhseph-svh;}else return 0;// 在信号发射时刻的基础上给定一个微小的时间间隔再次计算新时刻的 P、V、C。与3结合通过扰动法计算出卫星的速度和频漂。// 并没有使用那些位置和钟差公式对时间求导的结果/ satellite velocity and clock drift by differential approx */for (i0;i3;i) rsi3/tt; // 卫星速度rs[i3]dts1/tt; // 钟漂dts[1]return 1; }1. eph2pos() extern void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts,double *var) {double tk,M,E,Ek,sinE,cosE,u,r,i,O,sin2u,cos2u,x,y,sinO,cosO,cosi,mu,omge;double xg,yg,zg,sino,coso;int n,sys,prn;trace(4,eph2pos : time%s sat%2d\n,time_str(time,3),eph-sat);if (eph-A0.0) { //通过卫星轨道半长轴 A 判断星历是否有效无效则返回rs[0]rs[1]rs[2]*dts*var0.0;return;}tktimediff(time,eph-toe); //计算规化时间 tk (E.4.2)switch ((syssatsys(eph-sat,prn))) { //根据不同卫星系统设置相应的地球引力常数 mu 和地球自转角速度 omgecase SYS_GAL: muMU_GAL; omgeOMGE_GAL; break;case SYS_CMP: muMU_CMP; omgeOMGE_CMP; break;default: muMU_GPS; omgeOMGE; break;}Meph-M0(sqrt(mu/(eph-A*eph-A*eph-A))eph-deln)*tk; //计算平近点角 M (E.4.3)//用牛顿迭代法来计算偏近点角 E。参考 RTKLIB manual P145 (E.4.19) (E.4.4)for (n0,EM,Ek0.0;fabs(E-Ek)RTOL_KEPLERnMAX_ITER_KEPLER;n) {EkE; E-(E-eph-e*sin(E)-M)/(1.0-eph-e*cos(E));}if (nMAX_ITER_KEPLER) {trace(2,eph2pos: kepler iteration overflow sat%2d\n,eph-sat);return;}sinEsin(E); cosEcos(E);trace(4,kepler: sat%2d e%8.5f n%2d del%10.3e\n,eph-sat,eph-e,n,E-Ek);//计算摄动改正后的升交点角距u 卫星矢径长度r 轨道倾角iuatan2(sqrt(1.0-eph-e*eph-e)sinE,cosE-eph-e)eph-omg; //(E.4.5) (E.4.6) (E.4.10)reph-A(1.0-eph-e*cosE); //(E.4.11)ieph-i0eph-idot*tk; //(E.4.12)sin2usin(2.0*u); cos2ucos(2.0*u); ueph-cus*sin2ueph-cuc*cos2u; //(E.4.7)reph-crs*sin2ueph-crc*cos2u; //(E.4.8)ieph-cis*sin2ueph-cic*cos2u; //(E.4.9)xr*cos(u); yrsin(u); cosicos(i);// 北斗的MEO、IGSO卫星计算方法与GPS, Galileo and QZSS相同只是一些参数不同// GEO卫星的 O 和最后位置的计算稍有不同 / beidou geo satellite /if (sysSYS_CMP(prn5||prn59)) { / ref [9] table 4-1 */Oeph-OMG0eph-OMGd*tk-omge*eph-toes; //(E.4.29)sinOsin(O); cosOcos(O);xgx*cosO-y*cosi*sinO;ygx*sinOy*cosi*cosO;zgy*sin(i);sinosin(omge*tk); cosocos(omge*tk);rs[0] xg*coso yg*sino*COS_5 zg*sino*SIN_5; //ECEF位置(E.4.30)rs[1]-xg*sino yg*coso*COS_5 zg*coso*SIN_5;rs[2]-yg*SIN_5 zg*COS_5;}else {Oeph-OMG0(eph-OMGd-omge)*tk-omge*eph-toes; //计算升交点赤经O (E.4.13)sinOsin(O); cosOcos(O);rs[0]x*cosO-y*cosi*sinO; //计算卫星ECEF位置存入 rs 中 (E.4.14)rs[1]x*sinOy*cosi*cosO;rs[2]y*sin(i);}tktimediff(time,eph-toc); //(E.4.15)*dtseph-f0eph-f1*tkeph-f2*tktk; //利用三个二项式模型系数 af0、af1、af2计算卫星钟差/ relativity correction */ *dts-2.0*sqrt(mu*eph-A)*eph-esinE/SQR(CLIGHT); //相对论效应改正卫星钟差/ position and clock error variance */*varvar_uraeph(sys,eph-sva); //用 URA 值来标定方差 }2. var_uraeph()用URA用户测距精度标定卫星位置方差 GLONASS 不计算直接定位 55 static double var_uraeph(int sys, int ura) {const double ura_value[]{ 2.4,3.4,4.85,6.85,9.65,13.65,24.0,48.0,96.0,192.0,384.0,768.0,1536.0,3072.0,6144.0};if (sysSYS_GAL) { / galileo sisa (ref [7] 5.1.11) */if (ura 49) return SQR(ura*0.01);if (ura 74) return SQR(0.5(ura- 50)*0.02);if (ura 99) return SQR(1.0(ura- 75)*0.04);if (ura125) return SQR(2.0(ura-100)0.16);return SQR(STD_GAL_NAPA);}else { / gps ura (ref [1] 20.3.3.3.1.1) */return ura0||14ura?SQR(6144.0):SQR(ura_value[ura]);} }3. geph2pos()由 GLONASS 星历计算卫星位置钟差 GLONASS 卫星播发的是 PZ-90 坐标系下参考时刻的卫星状态向量每半个小时广播一次。如果需要得到某个时间的卫星位置必须通过运动模型积分得到。 extern void geph2pos(gtime_t time, const geph_t *geph, double *rs, double *dts,double *var) {double t,tt,x[6];int i;trace(4,geph2pos: time%s sat%2d\n,time_str(time,3),geph-sat);ttimediff(time,geph-toe);*dts-geph-taungeph-gamn*t; // 计算钟差dts(E.4.26)for (i0;i3;i) {x[i ]geph-pos[i];x[i3]geph-vel[i];}//步长 TSTEP:60sfor (ttt0.0?-TSTEP:TSTEP;fabs(t)1E-9;t-tt) {if (fabs(t)TSTEP) ttt;glorbit(tt,x,geph-acc);}for (i0;i3;i) rs[i]x[i];*varSQR(ERREPHGLO); // glonass卫星的方差直接定为 5*5 }4. glorbit()龙格库塔迭代 y n 1 y n h 6 ( k 1 2 k 2 2 k 3 k 4 ) k 1 f ( y n ) k 2 f ( y n k 1 h 2 ) k 3 f ( y n k 2 h 2 ) k 4 f ( y n k 3 h ) \begin{aligned} \mathrm{y}{\mathrm{n}1} \mathrm{y}{\mathrm{n}}\frac{\mathrm{h}}{6}\left(\mathrm{k}{1}2 \mathrm{k}{2}2 \mathrm{k}{3}\mathrm{k}{4}\right) \ \mathrm{k}{1} \mathrm{f}\left(\mathrm{y}{\mathrm{n}}\right) \ \mathrm{k}{2} \mathrm{f}\left(\mathrm{y}{\mathrm{n}}\mathrm{k}{1} \frac{\mathrm{h}}{2}\right) \ \mathrm{k}{3} \mathrm{f}\left(\mathrm{y}{\mathrm{n}}\mathrm{k}{2} \frac{\mathrm{h}}{2}\right) \ \mathrm{k}{4} \mathrm{f}\left(\mathrm{y}{\mathrm{n}}\mathrm{k}{3} \mathrm{~h}\right)\end{aligned} yn1k1k2k3k4yn6h(k12k22k3k4)f(yn)f(ynk12h)f(ynk22h)f(ynk3 h)
deq()微分方程计算 d x d t v x , d y d t v y , d z d t v z d v x d t − μ r 3 x − 3 2 J 2 μ a e 2 r 5 x ( 1 − 5 z 2 r 2 ) ω e 2 x 2 ω e v y a x d v y d t − μ r 3 y − 3 2 J 2 μ a e 2 r 5 y ( 1 − 5 z 2 r 2 ) ω e 2 y − 2 ω e v x a y d v z d t − μ r 3 z − 3 2 J 2 μ a e 2 r 5 z ( 3 − 5 z 2 r 2 ) a z \begin{array}{l}\frac{d x}{d t}v{x}, \frac{d y}{d t}v{y}, \frac{d z}{d t}v{z} \ \frac{d v{x}}{d t}-\frac{\mu}{r^{3}} x-\frac{3}{2} J{2} \frac{\mu a{e}^{2}}{r^{5}} x\left(1-\frac{5 z^{2}}{r^{2}}\right)\omega{e}^{2} x2 \omega{e} v{y}a{x} \ \frac{d v{y}}{d t}-\frac{\mu}{r^{3}} y-\frac{3}{2} J{2} \frac{\mu a{e}^{2}}{r^{5}} y\left(1-\frac{5 z^{2}}{r^{2}}\right)\omega{e}^{2} y-2 \omega{e} v{x}a{y} \ \frac{d v{z}}{d t}-\frac{\mu}{r^{3}} z-\frac{3}{2} J{2} \frac{\mu a{e}^{2}}{r^{5}} z\left(3-\frac{5 z^{2}}{r^{2}}\right)a{z}\end{array} dtdxvx,dtdyvy,dtdzvzdtdvx−r3μx−23J2r5μae2x(1−r25z2)ωe2x2ωevyaxdtdvy−r3μy−23J2r5μae2y(1−r25z2)ωe2y−2ωevxaydtdvz−r3μz−23J2r5μae2z(3−r25z2)az 其中 a e a{e} ae : earth semi-major axis ( 6378136.0 m ) (6378136.0 \mathrm{~m}) (6378136.0 m) μ \mu μ : earth gravitational constant ( 398600.44 × 1 0 9 m 3 / s 2 ) \left(398600.44 \times 10^{9} \mathrm{~m}^{3} / \mathrm{s}^{2}\right) (398600.44×109 m3/s2) ω e \omega{e} ωe : earth angular velocity ( 7.292115 × 1 0 − 5 r a d / s ) \left(7.292115 \times 10^{-5} \mathrm{rad} / \mathrm{s}\right) (7.292115×10−5rad/s) J 2 J{2} J2 : second zonal harmonic of the geopotential ( 1082625.7 × 1 0 − 9 ) \left(1082625.7 \times 10^{-9}\right) (1082625.7×10−9) r x 2 y 2 z 2 r\sqrt{x^{2}y^{2}z^{2}} rx2y2z2 4、peph2pos()精密星历计算卫星位置、钟差、速度、钟漂调用 pephpos() 根据精密星历计算卫星位置其中先二分查找时间最接近的精密星历然后地球自转改正调用 interppol() 内维尔插值获取卫星位置、线性插值获取钟差最后计算标准差。调用 pephclk() 根据精密星历计算卫星钟差其中先二分查找时间最接近的精密钟差再线性插值获取钟差、计算标准差。计算相对论效应改正量调用 satantoff() 计算卫星天线相位偏差改正。加上改正量得到卫星位置钟差。加上一个极小的时间再计算一次位置两次计算出的时间作差求得卫星速度钟飘。调用 satantoff() 天线相位中心改正。钟差做相对论效应改正 d T s ( t ) ( t i 1 − t ) d T s ( t i ) ( t − t i ) d T s ( t i 1 ) t i 1 − t i − 2 r s ( t ) T v s ( t ) c 2 d T^{s}(t)\frac{\left(t{i1}-t\right) d T^{s}\left(t{i}\right)\left(t-t{i}\right) d T^{s}\left(t{i1}\right)}{t{i1}-t{i}}-2 \frac{\boldsymbol{r}^{s}(t)^{T} \boldsymbol{v}^{s}(t)}{c^{2}} dTs(t)ti1−ti(ti1−t)dTs(ti)(t−ti)dTs(ti1)−2c2rs(t)Tvs(t)
extern int peph2pos(gtime_t time, int sat, const nav_t *nav, int opt,double *rs, double *dts, double var) {double rss[3],rst[3],dtss[1],dtst[1],dant[3]{0},vare0.0,varc0.0,tt1E-3;int i;if (sat0||MAXSATsat) return 0;// 调用 pephpos() 根据精密星历计算卫星位置// 调用 pephclk() 根据精密星历计算卫星钟差/ satellite position and clock bias /if (!pephpos(time,sat,nav,rss,dtss,vare,varc)||!pephclk(time,sat,nav,dtss,varc)) return 0;// 加上一个极小的时间再计算一次位置两次计算出的时间作差求得卫星速度钟飘timetimeadd(time,tt);if (!pephpos(time,sat,nav,rst,dtst,NULL,NULL)||!pephclk(time,sat,nav,dtst,NULL)) return 0;// 调用 satantoff() 天线相位中心改正/ satellite antenna offset correction /if (opt) {satantoff(time,rss,sat,nav,dant);}for (i0;i3;i) {rs[i ]rss[i]dant[i];rsi3/tt;}// 钟差做相对论效应改正/ relativistic effect correction */if (dtss[0]!0.0) {dts[0]dtss[0]-2.0dot(rs,rs3,3)/CLIGHT/CLIGHT;dts1/tt;}else / no precise clock */dts[0]dts[1]0.0;*varvarevarc;return 1; }1. 精密星历读取流程 nav-peph[] 存精密星历数据nav-ne 精密钟差数量。 nav-pclk[] 存精密钟差数据nav-nc 精密钟差数量。 execses_b() 中调用readpreceph()。readpreceph() 中readsp3()读取精密星历readrnxc() 读取精密钟差readsp3() 中readsp3h() 读文件头readsp3b() 读文件体combpeph() 对精密星历按时间、index 排序再将相同星历合并。readrnxc() 中readrnxfile() 读取精密星历文件combpclk() 排序合并精密钟差。
pephpos()精密星历计算卫星位置钟差执行流程如下 3. interppol()Neville 插值 Neville 算法是一种计算插值多项式方法由给定的 n1个节点存在一个唯一的幂次 ≤n 的多项式存在并且通过给定点所以可以由两个 n-1 次插值多项式构造一个 n 次多项式的线性逐次插值。给定 n 1 \mathrm{n}1 n1 个节点及其对应函数值 ( x i , y i ) \left(x{i}, y{i}\right) (xi,yi) 假设 P i , j P{i, j} Pi,j 表示 j − i j-i j−i 阶多项式并且满足通过节点 ( x k , y k ) k i , i 1 , ⋯ , j \left(x{k}, y{k}\right) \quad ki, i1, \cdots, j (xk,yk)ki,i1,⋯,j 。 P i , j P{i, j} Pi,j 满足以下迭代关系 p i , i ( x ) y i P i , j ( x ) ( x j − x ) p i , j − 1 ( x ) ( x − x i ) p i 1 , j ( x ) x j − x i , 0 ≤ i ≤ j ≤ n \begin{array}{l} p{i, i}(x)y{i} \ P{i, j}(x)\frac{\left(x{j}-x\right) p{i, j-1}(x)\left(x-x{i}\right) p{i1, j}(x)}{x{j}-x{i}}, \quad 0 \leq i \leq j \leq n \end{array} pi,i(x)yiPi,j(x)xj−xi(xj−x)pi,j−1(x)(x−xi)pi1,j(x),0≤i≤j≤n 以 n 4 n4 n4 的节点举例其迭代过程为 p 1 , 1 ( x ) y 1 p 2 , 2 ( x ) y 2 , p 1 , 2 ( x ) p 3 , 3 ( x ) y 3 , p 2 , 3 ( x ) , p 1 , 3 ( x ) p 4 , 4 ( x ) y 4 , p 3 , 4 ( x ) , p 2 , 4 ( x ) , p 1 , 4 ( x ) \begin{array}{l} p{1,1}(x)y{1} \ p{2,2}(x)y{2}, p{1,2}(x) \ p{3,3}(x)y{3}, p{2,3}(x), p{1,3}(x) \ p{4,4}(x)y{4}, p{3,4}(x), p{2,4}(x), p{1,4}(x) \end{array} p1,1(x)y1p2,2(x)y2,p1,2(x)p3,3(x)y3,p2,3(x),p1,3(x)p4,4(x)y4,p3,4(x),p2,4(x),p1,4(x) static double interppol(const double *x, double *y, int n) {int i,j;for (j1;jn;j) {for (i0;in-j;i) {yi/(x[ij]-x[i]);}}return y[0]; }4. posWithEarhRotation() static void posWithEarhRotation(const int k, double pos[3], double p[3][NMAX1], double dt) {double sinl,cosl; #if 0p[0][k]pos[0];p[1][k]pos[1]; #else/* correciton for earh rotation ver.2.4.0 */sinlsin(OMGE*dt);coslcos(OMGE*dt);p[0][k]cosl*pos[0]-sinl*pos[1];p[1][k]sinl*pos[0]cosl*pos[1]; #endifp[2][k]pos[2]; }5. pephclk()精密钟差计算卫星钟差简单的线性插值 d T s ( t ) ( t i 1 − t ) d T s ( t i ) ( t − t i ) d T s ( t i 1 ) t i 1 − t i d T^{s}(t)\frac{\left(t{i1}-t\right) d T^{s}\left(t{i}\right)\left(t-t{i}\right) d T^{s}\left(t{i1}\right)}{t{i1}-t{i}} dTs(t)ti1−ti(ti1−t)dTs(ti)(t−ti)dTs(ti1) IGS 的精密钟差计算完之后需要考虑相对论效应的影响 d T s ( t ) ( t i 1 − t ) d T s ( t i ) ( t − t i ) d T s ( t i 1 ) t i 1 − t i − 2 r s ( t ) T v s ( t ) c 2 d T^{s}(t)\frac{\left(t{i1}-t\right) d T^{s}\left(t{i}\right)\left(t-t{i}\right) d T^{s}\left(t{i1}\right)}{t{i1}-t_{i}}-2 \frac{\boldsymbol{r}^{s}(t)^{T} \boldsymbol{v}^{s}(t)}{c^{2}} dTs(t)ti1−ti(ti1−t)dTs(ti)(t−ti)dTs(ti1)−2c2rs(t)Tvs(t) 三、rescode()残差计算、设计矩阵构建计算当前迭代的伪距残差 v、设计矩阵 H、伪距残差的方差 var、所有观测卫星的方位角和仰角 azel定位时有效性 vsat、定位后伪距残差 resp、参与定位的卫星个数 ns 和方程个数 nv int iter I 迭代次数在estpos()里迭代调用第i次迭代就传i int bElevCVG I obsd_t *obs I 观测量数据 int n I 观测量数据的数量 double *rs I 卫星位置和速度长度为6*n{x,y,z,vx,vy,vz}(ecef)(m,m/s) double *dts I 卫星钟差长度为2*n {bias,drift} (s|s/s) double *vare I 卫星位置和钟差的协方差 (m^2) int *svh I 卫星健康标志 (-1:correction not available) nav_t *nav I 导航数据 double *x I 本次迭代开始之前的定位值,7*1,前3个是本次迭代开始之前的定位值第4个是钟差后三个分别是gps系统与glonass、galileo、bds系统的钟差。 prcoptt *opt I 处理过程选项 double *v O 定位方程的右端部分伪距残差 double *H O 定位方程中的几何矩阵 double *var O 参与定位的伪距残差的方差 double *azel O 对于当前定位值所有观测卫星的 {方位角、高度角} (2*n) int *vsat O 所有观测卫星在当前定位时是否有效 (1*n) double *resp O 所有观测卫星的伪距残差(P-(rc*dtr-c*dtsIT)) (1*n) int *ns O 参与定位的卫星的个数 int *bDeleted O 将之前得到的定位解信息赋值给rr和dtr数组。调用ecef2pos()将将接收机位置rr由 ECEF-XYZ 转换为大地坐标系LLHpos 遍历当前历元所有OBS[]即遍历每颗卫星将vsat[]、azel[]和resp[]数组置 0因为在前后两次定位结果中每颗卫星的上述信息都会发生变化。time赋值OBS的时间sat赋值OBS的卫星。检测当前观测卫星是否和下一个相邻数据重复重复则不处理这一条continue去处理下一条。调用satexclude()函数判断卫星是否需要排除如果排除则continue去处理下一个卫星。调用geodist()函数计算卫星和当前接收机位置之间的几何距离r和接收机到卫星的方向向量e。调用satazel()函数计算在接收机位置处的站心坐标系中卫星的方位角和仰角若仰角低于截断值opt-elmincontinue不处理此数据。调用snrmask()根据接收机高度角和信号频率来检测该信号是否可用。调用 ionocorr() 函数计算电离层延时I,所得的电离层延时是建立在 L1 信号上的当使用其它频率信号时依据所用信号频组中第一个频率的波长与 L1 波长的比率对上一步得到的电离层延时进行修正。调用tropcorr()函数,计算对流层延时T。调用prange()函数计算经过DCB校正后的伪距值p。计算伪距残差v[nv]即经过钟差对流层电离层改正后的伪距。组装设计矩阵H h ( x ) ( ρ r 1 c d t r − c d T 1 I r 1 T r 1 ρ r 2 c d t r − c d T 2 I r 2 T r 2 ρ r 3 c d t r − c d T 3 I r 3 T r s 3 ⋮ ρ r m c d t r − c d T m I r m T r m ) H ( − e r 1 T 1 − e r 2 T 1 − e r 3 T 1 ⋮ ⋮ − e r m T 1 ) \boldsymbol{h}(\boldsymbol{x})\left(\begin{array}{c}\rho{r}^{1}c d t{r}-c d T^{1}I{r}^{1}T{r}^{1} \ \rho{r}^{2}c d t{r}-c d T^{2}I{r}^{2}T{r}^{2} \ \rho{r}^{3}c d t{r}-c d T^{3}I{r}^{3}T{r}^{s 3} \ \vdots \ \rho{r}^{m}c d t{r}-c d T^{m}I{r}^{m}T{r}^{m}\end{array}\right) \boldsymbol{H}\left(\begin{array}{cc}-e{r}^{1 T} 1 \ -e{r}^{2 T} 1 \ -e{r}^{3 T} 1 \ \vdots \vdots \ -e_{r}^{m T} 1\end{array}\right) h(x) ρr1cdtr−cdT1Ir1Tr1ρr2cdtr−cdT2Ir2Tr2ρr3cdtr−cdT3Ir3Trs3⋮ρrmcdtr−cdTmIrmTrm H −er1T−er2T−er3T⋮−ermT111⋮1 处理不同系统GPS、GLO、GAL、CMP之间的时间偏差修改矩阵H 。调用varerr()函数计算此时的导航系统误差为了防止不满秩的情况把矩阵H补满秩了H[jnv*NX]ji3?1.0:0.0;
static int rescode(const int iter, int bElevCVG, const obsd_t *obs, int n, const double *rs, const double *dts, const double *vare, const int *svh, const nav_t *nav, const double *x, const prcopt_t *opt, double *v, double *H, double *var, double *azel, int *vsat, double *resp, int *nx, int *bDeleted) {int bObserved[5];int i,j,nv0,ns[5]{0},sys,satsn[MAXOBS],sat;double r,dion,dtrp,vion,vtrp,rr[3],pos[3],dtr,e[3],P,elev_t[MAXSAT],vmeas,lam_L1;double elmin;int bMulGNSS;nxNX_SPP;for (i0;i5;i) {bObserved[i]0; ns[i]0;}for (i0;in;i) v[i]var[i]0.0;for (i0;iNX_SPP(n5);i) H[i]0.0;for (i0;iMAXSAT;i) elev_t[i]0.0;//将之前得到的定位解信息赋值给 rr 和 dtr 数组以进行关于当前解的伪距残差的相关计算for (i0;i3;i) rr[i]x[i]; dtrx[3];// rr{x,y,z}-pos{lat,lon,h} ecef2pos(rr,pos);// 遍历当前历元观测值for (i0;iniMAXOBS;i) {satobs[i].sat; // sat 赋值 OBS 的卫星vsat[i]0; azel[i*2]azel[1i2]resp[i]0.0;sysPPP_Glo.sFlag[sat-1].sys;// 如果这颗卫星 bDeleted 被排除了或者卫星系统没使用跳过当前观测值不处理if (bDeleted[sat-1]0) continue;if (!(sysopt-navsys)) continue;// 去除重复观测值/ reject duplicated observation data /if (in-1iMAXOBS-1obs[i].satobs[i1].sat) {sprintf(PPP_Glo.chMsg,** WARNING: duplicated observation data %s sat%2d\n,time_str(obs[i].time,3),obs[i].sat);outDebug(OUTWIN,OUTFIL,0);i;continue;}// 调用 geodist() 计算近似几何距离// 调用 satazel() 计算方位角高度角剔除高度角过低的卫星观测值/* geometric distance/azimuth/elevation angle */if ((rgeodist(rsi*6,rr,e))0.0) continue; // 近似几何距离小于 0排除satazel(pos,e,azeli*2);if (bElevCVG) {if (PPP_Glo.prcOpt_Ex.bElevCheckEx) { // 如果启用了高度角检查截止高度角不能小于两度elmin0.0;elminMIN(opt-elmin,2.0*D2R);elminMAX(opt-elmin/3.0,elmin);if (azel[1i*2]elmin) continue; // 高度角小于设截止高度角排除}else {if (azel[1i*2]opt-elmin) continue;}}else {if (azel[1i*2]0.0)azel[1i*2]MIN(3.0*D2R,opt-elmin*0.75);if (iter1) {if (azel[1i2]opt-elmin) continue;}}// 调用 prange() 码偏差改正得到改正后的伪距 P如果设置了消电离层组合直接得到组合后的伪距 P/ psudorange with code bias correction */if ((Pprange(obsi,nav,azeli2,opt,vmeas))0.0) continue;// 调用 satexclude() 排除不可用卫星的观测值/ excluded satellite /if (satexclude(obs[i].sat,svh[i],opt)) continue;// 调用 ionocorr() 克罗布歇电离层改正计算 L1 信号上计算电离层延时 I// 当使用其它频率信号时依据所用信号频组中第一个频率的波长与 L1 波长的关系对上一步得到的电离层延时进行修正/ ionospheric corrections */if (!ionocorr(obs[i].time,sys,nav,pos,azeli2,opt,dion,vion)) continue;/ GPS-L1 - L1/B1 */if ((lam_L1nav-lam[obs[i].sat-1][0])0.0) {dionSQR(lam_L1/lam_carr[0]);}// 调用 tropcorr() Saastamoinen 对流层改正/ tropospheric corrections */if (!tropcorr(obs[i].time,nav,pos,azeli*2,opt,dtrp,vtrp)) continue;// 构建残差向量 V P-(rc*dtr-cdtsIT) (E.6.31)/ pseudorange residual */v[nv]resp[i]P-(rdtr-CLIGHT*dts[i2]diondtrp);// 构建设计矩阵 H前 3 行为中计算得到的视线向第 4 行为 1其它行为 0/ design matrix */for (j0;j4;j) H[jnvNX_SPP]j3?-e[j]:1.0;// 修改矩阵 H处理不同系统GPS、GLO、GAL、CMP之间的时间偏差/ time system and receiver bias offset */if (sysSYS_GLO) {v[nv]-x[4]; H[4nv*NX_SPP]1.0; ns[1]; bObserved[1]1;}else if (sysSYS_CMP) {v[nv]-x[5]; H[5nv*NX_SPP]1.0; ns[2]; bObserved[2]1;}else if (sysSYS_GAL) {v[nv]-x[6]; H[6nv*NX_SPP]1.0; ns[3]; bObserved[3]1;}else if (sysSYS_QZS) {v[nv]-x[7]; H[7nv*NX_SPP]1.0; ns[4]; bObserved[4]1;}else {H[4nv*NX_SPP]H[5nv*NX_SPP]0.0; ns[0]; bObserved[0]1;}vsat[i]1; resp[i]v[nv];satsn[nv]obs[i].sat;elev_t[sat-1]azel[1i*2]R2D;// 调用 varerr() 函数计算此时的导航系统误差然后累加计算用户测距误差(URE)/ error variance */var[nv]varerr(opt,azel[1i*2],sys)vare[i]vionvtrp;if (azel[1i*2]opt-elmin) var[nv]*100.0;nv;}for (ij0;i5;i) {if (bObserved[i]) j;}bMulGNSS0;if (j2) bMulGNSS1;// 调用 getHVR_spp()获取 HVRinv;nvgetHVR_spp(bMulGNSS,iter,opt-navsys,bElevCVG,bDeleted,satsn,H,v,var,elevt,nv,nx);for (i0;i5;i) {if (ns[i]0) continue;for (j0;jnv;j) H[(3i)j(*nx)]0.0;}for (i0;i5;i) {if (ns[i]0) *nx*nx-1;}//*nx*nx-1;return nv;// v 和 resp 的主要区别在于长度不一致// v 是需要参与定位方程组的解算的维度为 nv*1// resp 仅表示所有观测卫星的伪距残余维度为 n*1对于没有参与定位的卫星该值为 0 }1、geodist()计算近似几何距离、接收机到卫星方向的观测矢量sagnac 效应改正地球自转引起的误差是信号传输过程中 GNSS 卫星信号发射时刻和接收机接收到信号的时刻之间地球自转对 GNSS 观测值产生的影响。相当于地球自转使得卫星空间位置在信号播发、收到的过程中接收机在地固系坐标轴上相对于 Z \mathrm{Z} Z 轴发生了一定角度的旋转使得 GNSS 卫星的在信号发射时刻的位置发生了变化也称为 Sagnac 效应。地球自转引起的距离改正公式如下 [ x s ′ y s s ′ z s ′ ] [ cos ⁡ ω τ sin ⁡ ω τ 0 − sin ⁡ ω τ cos ⁡ ω τ 0 0 0 1 ] [ x s y s z s ] δ sagnac, r , j ω e c ( x s y r − y s x r ) \begin{array}{c}{\left[\begin{array}{l}x^{s^{\prime}} \ y^{s^{s^{\prime}}} \ z^{s^{\prime}}\end{array}\right]\left[\begin{array}{ccc}\cos \omega \tau \sin \omega \tau 0 \ -\sin \omega \tau \cos \omega \tau 0 \ 0 0 1\end{array}\right]\left[\begin{array}{l}x^{s} \ y^{s} \ z^{s}\end{array}\right]} \ \delta{\text {sagnac, } r, j}\frac{\omega{e}}{c}\left(x^{s} y{r}-y^{s} x{r}\right)\end{array} xs′yss′zs′ cosωτ−sinωτ0sinωτcosωτ0001 xsyszs δsagnac, r,jcωe(xsyr−ysxr) 上式中 ( x s ′ , y s ′ , z s ′ ) \left(x^{s^\prime}, y^{s^{\prime}}, z^{s^{\prime}}\right) (xs′,ys′,zs′) 为地球自旋转后卫星的坐标值, ( x s , y s , z s ) \left(x^{s}, y^{s}, z^{s}\right) (xs,ys,zs) 为地球自旋转前卫星的坐标值, ω e \omega{e} ωe 代表地球自传角速度值, τ \tau τ 为卫星发射信号时刻到接收机接收卫星时刻的历元数。改正地球自转后的近似几何距离近似几何距离如下 ρ r s ≈ ∥ r r ( t r ) − r s ( t s ) ∥ ω e c ( x s y r − y s x r ) \begin{array}{l} \rho{r}^{s} \approx\left|\boldsymbol{r}{r}\left(t{r}\right)-\boldsymbol{r}^{s}\left(t^{s}\right)\right|\frac{\omega{e}}{c}\left(x^{s} y{r}-y^{s} x{r}\right) \end{array} ρrs≈∥rr(tr)−rs(ts)∥cωe(xsyr−ysxr) 接收机到卫星方向的观测矢量计算公式如下 e r s r s ( t s ) − r r ( t r ) ∥ r s ( t s ) − r r ( t r ) ∥ \boldsymbol{e}{r}^{s}\frac{\boldsymbol{r}^{s}\left(t^{s}\right)-\boldsymbol{r}{r}\left(t{r}\right)}{\left|\boldsymbol{r}^{s}\left(t^{s}\right)-\boldsymbol{r}{r}\left(t_{r}\right)\right|} ers∥rs(ts)−rr(tr)∥rs(ts)−rr(tr) 对应的代码如下传入 ECEF 卫星坐标 rs、接收机近似 ECEF 坐标 rr计算之后近似几何距离做返回值返回观测矢量做参数三 e 返回 extern double geodist(const double *rs, const double *rr, double e){double r;int i;if (norm(rs,3)RE_WGS84) return -1.0; // 检查卫星到 WGS84坐标系原点的距离是否大于基准椭球体的长半径。for (i0;i3;i) e[i]rs[i]-rr[i]; // 求卫星和接收机坐标差e[]rnorm(e,3); // 求未经萨格纳克效应改正的距离for (i0;i3;i) e[i]/r; // 接收机到卫星的单位向量ereturn rOMGE(rs[0]*rr[1]-rs[1]*rr[0])/CLIGHT; //(E.3.8b)}2、satazel()计算方位角高度角方位角范围在 [ 0 , 2 π ] [0,2 \pi] [0,2π]高度角范围在 [ − π 2 , π 2 ] [-\frac{\pi}{2},\frac{\pi}{2}] [−2π,2π]以接收机为原点建立站心坐标系 ENU根据卫星 ENU 下方向矢量可以得到高度角、方位角公式如下 e r , enu s E r e r s ( e e , e n , e u ) T A z r s ATAN ⁡ 2 ( e e , e n ) E l r s arcsin ⁡ ( e u ) \begin{array}{l}\boldsymbol{e}{r, \text { enu }}^{s}\boldsymbol{E}{r} \boldsymbol{e}{r}^{s}\left(e{e}, e{n}, e{u}\right)^{T} \ A z{r}^{s}\operatorname{ATAN} 2\left(e{e}, e{n}\right) \ E l{r}^{s}\arcsin \left(e_{u}\right)\end{array} er, enu sErers(ee,en,eu)TAzrsATAN2(ee,en)Elrsarcsin(eu) 对应的代码如下传入接收机 LLH 坐标 pos、接收机到卫星方向的观测矢量 e计算之后返回弧度制的高度角如果传了参三 azel那么 azel[0] 是方位角、azel[1] 是高度角。 extern double satazel(const double *pos, const double *e, double *azel) {double az0.0,elPI/2.0,enu[3];if (pos[2]-REWGS84) {ecef2enu(pos,e,enu);azdot(enu,enu,2)1E-12?0.0:atan2(enu[0],enu[1]);if (az0.0) az2*PI;elasin(enu[2]);}if (azel) {azel[0]az; azel[1]el;}return el; }3、prange()差分码偏差改正 DCB 差分码偏差针对伪距是由不同类型的 GNSS 信号在卫星和接收机不同通道产生的时间延迟硬件延迟码偏差差异。由于卫星播发的测距码类型很多 C1、 P1、 P2 等不同的测距信号虽然在同一台卫星钟的驱动下生成的因而花费的时间也不同。我们把卫星钟脉冲驱动下开始生成测距信号至信号生成并最终离开卫星发射天线相位中心之间所花费的时间称为信号在卫星内部的时延。DCB 体现的就是不同码信号时延的差。分为频内偏差相同频率不同码之间存在的偏差如 P1-C1、P2-C2 等频间偏差不同频率之间存在的偏差如 P1-P2 一般来说接收机端的DCB被接收机钟差所吸收可以跟接收机钟差一起解算。若需提高定位精度卫星端的码偏差需进行校正。目前码偏差产品主要分为 2 类① 广播星历播发的时间群延迟(time group delayTGD)产品② IGS分析中心提供的高精度后处理差分码偏差(differential code biasDCB)产品。因此TGD 产品相比于 DCB 产品精度低于 DCB 产品且适用于实时场景。DCB 与 TGD 直接计算的关系如下 T G D 1 1 − γ D C B 12 T G D\frac{1}{1-\gamma} D C B{12} TGD1−γ1DCB12 D C B 12 ( 1 − γ ) × T G D D C B_{12}(1-\gamma) \times T G D DCB12(1−γ)×TGD 对于 GPS 而言其广播星历及精密星历是采用 P1、P2 无电离层组合进行卫星钟差估计。因此广播星历钟差及精密星历钟差均包含 P1、P2 无电离层组合的硬件延迟。当用户基于 P1、P2 无电离层组合定位解算时无需考虑硬件延迟反之若用户使用 P1、P2 单频或其他组合时均需要考虑硬件延迟的影响否则会影响定位解算的精度。若用户使用 C/A 码用户需借助外部文件获取 P-C 将 C/A 码归化到 P 码然后再进行 TGD/DCB 改正。与 GPS 不同BDS 广播星历的钟差基准参考 B3 频点多数机构的精密钟差基准是 B1/B3 无电离层组合。 4、gettgd() static double gettgd(int sat, const nav_t *nav, double *tgd1, double *tgd2) {int i;for (i0;inav-n;i) {if (nav-eph[i].sat!sat) continue;// 如果是北斗if (PPP_Glo.sFlag[sat-1].sysSYS_CMP) {if (tgd1) *tgd1CLIGHT*nav-eph[i].tgd[0];if (tgd2) *tgd2CLIGHTnav-eph[i].tgd[1];return CLIGHT(nav-eph[i].tgd[0]);}// 群波延迟参数 * 光速return CLIGHT*nav-eph[i].tgd[0];}return 0.0; }4、satexclude()排除不可用卫星的观测值卫星健康标志 svh 0排除。根据 opt-exsats[sat-1] 判断是否使用了该卫星。根据 opt-navsys 判断是否使用了该卫星系统。 extern int satexclude(int sat, int svh, const prcopt_t opt) {int sysPPP_Glo.sFlag[sat-1].sys;// 卫星健康标志 svh 0排除if (svh0) return 1; / ephemeris unavailable /if (opt) {// 根据 opt-exsats[sat-1] 判断是否使用了该卫星if (opt-exsats[sat-1]1) return 1; / excluded satellite /if (opt-exsats[sat-1]2) return 0; / included satellite /// 根据 opt-navsys 判断是否使用了该卫星系统if (!(sysopt-navsys)) return 1; / unselected sat sys /}if (sysSYS_QZS) svh0xFE; / mask QZSS LEX health /if (svh) {sprintf(PPP_Glo.chMsg,** WARNING: unhealthy satellite: sat%3d svh%02X\n,sat,svh);outDebug(0,0,0);return 1;}return 0; }5、ionocorr()根据选项模型计算 L1 电离层延迟 I 受太阳辐射的影响距地面 60km 以上的大气层处于部分电离或完全电离状态该区域被称为电离层。当电磁波信号通过电离层时传播速度和传播路径会发生改变给 GNSS 观测值带来误差即电离层延迟。电离层延迟大小由电子密度和信号频率决定影响可达数十米。电离层延迟与单位面积的横截面在信号传播路径上拦截的电子总量 N e Ne Ne 成正比且与载波频率 f f f 的平方成反比弥散性的电离层降低了测距码的传播速度而加快了载波相位的传播速度如下所示 I I ρ − I ϕ 40.28 N e f 2 II \rho-I_\phi40.28\frac{N_e}{f^2} IIρ−Iϕ40.28f2Ne 在一些条件下可使用经验模型改正或约束观测值中的电离层延迟常用的电离层延迟经验模型有 Klobuchar 模型、Bent 模型和电离层格网模型等。可以总结出电离层几个对于解算有用的特点对伪距和载波影响相反UOFC 组合但伪距载波之间延迟与频率有关消电离层组合但放大噪声延迟与电子总量 TEC 成正比建立电离层格网模型 TEC 文件电离层改正或者将 STEC 作为参数估计。 static int ionocorr(gtime_t time, const int sys, const nav_t *nav, const double *pos,const double *azel, const prcopt_t *opt, double *ion,double var) {// 克罗布歇模型广播星历模型/ broadcast model */if (opt-ionooptIONOOPT_BRDC) {//*ionionmodel(time,nav-ion_gps,pos,azel);if (sysSYS_GPS) *ionionmodel(time,nav-ion_gps,pos,azel);//else if ( SYS_CMPsys ) *ionionmodel(time,nav-ion_cmp,pos,azel);else *ionionmodel(time,nav-ion_gps,pos,azel);*varSQR(*ionERR_BRDCI);return 1;}// 电离层格网模型/ ionex tec model */ else if (opt-ionooptIONOOPT_TEC) {return iontec(time,nav,pos,azel,3,ion,var);} // IF 消电离层组合else if (opt-ionooptIONOOPT_IF12) {*ion0.0;*varSQR(0.02);return 1;}*ion0.0;*varopt-ionooptIONOOPT_OFF?SQR(ERRION):0.0;return 1; }6、ionmodel()克罗布歇模型电离层延迟计算 SPP 中使用克罗布歇模型计算 L1 的电离层改正量将晚间的电离层时延视为常数取值为 5ns把白天的时延看成是余弦函数中正的部分。于是天顶方向调制在 L1 载波上的测距码的电离层时延可表示为 T g 5 × 1 0 − 9 A cos ⁡ 2 π P ( t − 1 4 h ) T{g}5 \times 10^{-9}A \cos \frac{2 \pi}{P}\left(t-14^{h}\right) Tg5×10−9AcosP2π(t−14h) 振幅 A A A 和周期 P P P 分别为 A ∑ i 0 3 α i ( φ m ) i P ∑ i 0 3 β i ( φ m ) i \begin{array}{l} A\sum{i0}^{3} \alpha{i}\left(\varphi{m}\right)^{i} \ P\sum{i0}^{3} \beta{i}\left(\varphi{m}\right)^{i} \end{array} A∑i03αi(φm)iP∑i03βi(φm)i 全球定位系统向单频接收机用户提供的电离层延迟改正时就采用上述模型。其中 α i \alpha_i αi 和 β i \betai βi 是地面控制系统根据该天为一年中的第几天将一年分为 37 个区间以及前 5 天太阳的平均辐射流量共分10档从 370 组常数中选取的然后编入星导航电文播发给用户。卫星在其播发的导航电文中提供了这 8 个电离层延迟参数 p ion ( α 0 , α 1 , α 2 , α 3 , β 0 , β 1 , β 2 , β 3 ) T p{\text {ion }}\left(\alpha{0}, \alpha{1}, \alpha{2}, \alpha{3}, \beta{0}, \beta{1}, \beta{2}, \beta{3}\right)^{T} pion (α0,α1,α2,α3,β0,β1,β2,β3)T 根据根据参数 α 0 α 1 α 2 α 3 \alpha_0\alpha_1\alpha_2\alpha_3 α0α1α2α3 确定振幅 A A A根据根据参数 β 0 β 1 β 2 β 3 \beta_0\beta_1\beta_2\beta3 β0β1β2β3 确定周期 T T T再给定一个以秒为单位的当地时间 t t t就能算出天顶电离层延迟。再由天顶对流层延迟根据倾斜率转为卫星方向对流层延迟。电离层分布在离地面 60-1000km 的区域内。当卫星不在测站的天顶时信号传播路径上每点的地方时和纬度均不相同为了简化计算我们将整个电离层压缩为一个单层将整个电离层中的自由电子都集中在该单层上用它来代替整个电离层。这个电离层就称为中心电离层。中心电离层离地面的高度通常取 350km。式中的参数 t t t 和式中的参数 φ m \varphi{m} φm 分别为卫星言号传播路径与中心电离层的交点 P ′ P^{\prime} P′ 的时角和地磁纬度因为只有 P ′ P^{\prime} P′ 才能反映卫星信号所受到的电离层延迟的总的情况。综上已知大地经度、大地纬度、卫星的高度角和卫星测站的方位角电离层延迟计算方法如下计算测站 P P P 和 P ′ P^{\prime} P′ 在地心的夹角 ψ 0.0137 / ( E l 0.11 ) − 0.022 \psi0.0137 /(E l0.11)-0.022 ψ0.0137/(El0.11)−0.022 计算交点 P ′ P^{\prime} P′ 的地心纬度 φ i φ ψ cos ⁡ A z \varphi{i}\varphi\psi \cos A z φiφψcosAz { φ l 0.416 φ l 0.416 φ l − 0.416 φ l − 0.416 \left{\begin{array}{ll}\varphi{l}0.416 \varphi{l}0.416 \ \varphi{l}-0.416 \varphi{l}-0.416\end{array}\right. {φl0.416φl−0.416φl0.416φl−0.416 计算交点 P ′ P^{\prime} P′ 的地心经度 λ i λ ψ sin ⁡ A z / cos ⁡ φ i \lambda{i}\lambda\psi \sin A z / \cos \varphi{i} λiλψsinAz/cosφi 计算地磁纬度 φ m φ i 0.064 cos ⁡ ( λ i − 1.617 ) \varphi{m}\varphi{i}0.064 \cos \left(\lambda{i}-1.617\right) φmφi0.064cos(λi−1.617) 计算观测瞬间交点 P ′ P^{\prime} P′ 处的地方时 t 4.32 × 1 0 4 λ i t t4.32 \times 10^{4} \lambda{i}t t4.32×104λit { t 86400 t t − 86400 t 0 t t 86400 \left{\begin{array}{ll}t86400 tt-86400 \ t0 tt86400\end{array}\right. {t86400t0tt−86400tt86400 计算倾斜因子 F 1.0 16.0 × ( 0.53 − E l ) 3 F1.016.0 \times(0.53-E l)^{3} F1.016.0×(0.53−El)3 计算电离层时间延迟 x 2 π ( t − 50400 ) / ∑ n 0 3 β n φ m n I r s { F × 5 × 1 0 − 9 F × ( 5 × 1 0 − 9 ∑ n 1 4 α n φ m n × ( 1 − x 2 2 x 4 24 ) ) ( ∣ x ∣ 1.57 ) \begin{array}{l}x2 \pi(t-50400) / \sum{n0}^{3} \beta{n} \varphi{m}{ }^{n} \ I{r}^{s}\left{\begin{array}{cc}F \times 5 \times 10^{-9} \ F \times\left(5 \times 10^{-9}\sum{n1}^{4} \alpha{n} \varphi{m}{ }^{n} \times\left(1-\frac{x^{2}}{2}\frac{x^{4}}{24}\right)\right) (|x|1.57)\end{array}\right.\end{array} x2π(t−50400)/∑n03βnφmnIrs{F×5×10−9F×(5×10−9∑n14αnφmn×(1−2x224x4))(∣x∣1.57) 计算的是 L1 信号的电离层延时 I 当使用其它频率信号时依据所用信号频组中第一个频率的波长与 L1 波长的比例关系对上一步得到的电离层延时进行修正不考虑模糊度情况下改正公式为 I Φ 2 − Φ 1 1 − ( f 1 / f 2 ) 2 I\frac{\Phi{2}-\Phi{1}}{1-\left(f{1} / f_{2}\right)^{2}} I1−(f1/f2)2Φ2−Φ1 extern double ionmodel(gtime_t t, const double *ion, const double *pos,const double azel) {const double ion_default[]{ / 2004/1/1 /0.1118E-07,-0.7451E-08,-0.5961E-07, 0.1192E-06,0.1167E06,-0.2294E06,-0.1311E06, 0.1049E07};double tt,f,psi,phi,lam,amp,per,x;int week;if (pos[2]-1E3||azel[1]0) return 0.0;if (norm(ion,8)0.0) ionion_default; //若没有电离层参数用默认参数/ earth centered angle (semi-circle) / //地球中心角psi0.0137/(azel[1]/PI0.11)-0.022; //计算地心角(E.5.6)/ subionospheric latitude/longitude (semi-circle) */ phipos[0]/PIpsi*cos(azel[0]); //计算穿刺点地理纬度(E.5.7)if (phi 0.416) phi 0.416; //phi不超出(-0.416,0.416)范围else if (phi-0.416) phi-0.416;lampos[1]/PIpsi*sin(azel[0])/cos(phiPI); //计算穿刺点地理经度(E.5.8)/ geomagnetic latitude (semi-circle) */phi0.064*cos((lam-1.617)PI); //计算穿刺点地磁纬度(E.5.9)/ local time (s) */tt43200.0*lamtime2gpst(t,week); //计算穿刺点地方时(E.5.10)tt-floor(tt/86400.0)86400.0; / 0tt86400 // slant factor */f1.016.0pow(0.53-azel[1]/PI,3.0); //计算投影系数(E.5.11)/ ionospheric delay /ampion[0]phi(ion[1]phi*(ion[2]phiion[3]));perion[4]phi(ion[5]phi*(ion[6]phi*ion[7]));ampamp 0.0? 0.0:amp;perper72000.0?72000.0:per;x2.0PI(tt-50400.0)/per; //(E.5.12)return CLIGHTf(fabs(x)1.57?5E-9amp*(1.0xx(-0.5x*x/24.0)):5E-9); //(E.5.13) }7、tropcorr()对流层改正对流层一般指距离地面 50km 内的大气层是大气层质量的主要部分。当导航信号穿过对流层时由于传播介质密度的增加信号传播路径和传播速度会发生改变由此引起的 GNSS 观测值误差称为对流层延迟。对流层延迟一般可分为干延迟和湿延迟对于载波相位和伪距完全相同一般在米级大小可通过模型改正和参数估计的方法来削弱其影响。修正模型如下 T M d r y T d r y M w e t T w e t TM{d r y} T{d r y}M{w e t} T{w e t} TMdryTdryMwetTwet 式中 T d r y , T w e t T{d r y}, T{w e t} Tdry,Twet 分别表示接收机天顶对流层的干延迟和湿延迟 M d r y , M w e t M{d r y}, M{w e t} Mdry,Mwet 分别表示干延迟和湿延迟的投影函数。对流层干延迟比较稳定主要与测站高度、大气温度和大气压相关可通过模型改正常用模型有 Saastamoninen 模型、Hopfield 模型等。湿延迟不同于干延迟变化较大主要与水汽含量相关一般估计天顶对流层湿延迟通过投影函数计算各卫星的电离层湿延迟常用的投影函数有全球投影函数Global Mapping FunctionGMF、Niell 投影函数NMF和 Vienna投影函数Vienna Mapping FunctionVMF等。可以总结出电离层几个对于解算有用的特点非弥散介质对所有卫星的信号影响相同。分干湿延迟 8、tropmodel()Saastamoinen 模型计算对流层延迟 p 1013.25 × ( 1 − 2.2557 × 1 0 − 5 h ) 5.2568 T 15.0 − 6.5 × 1 0 − 3 h 273.15 e 6.108 × exp ⁡ { 17.15 T − 4684.0 T − 38.45 } × h r e l 100 \begin{array}{l}p1013.25 \times\left(1-2.2557 \times 10^{-5} h\right)^{5.2568} \ T15.0-6.5 \times 10^{-3} h273.15 \ e6.108 \times \exp \left{\frac{17.15 T-4684.0}{T-38.45}\right} \times \frac{h{r e l}}{100}\end{array} p1013.25×(1−2.2557×10−5h)5.2568T15.0−6.5×10−3h273.15e6.108×exp{T−38.4517.15T−4684.0}×100hrel T r s 0.002277 cos ⁡ z { p ( 1255 T 0.05 ) e − tan ⁡ 2 z } T{r}^{s}\frac{0.002277}{\cos z}\left{p\left(\frac{1255}{T}0.05\right) e-\tan ^{2} z\right} Trscosz0.002277{p(T12550.05)e−tan2z} 其中 z z z 是天顶角与高度角互余 z π / 2 − E l r s z\pi / 2-E l{r}^{s} zπ/2−Elrs 7、varerr()计算伪距量测协方差 σ 2 F s R r ( a σ 2 b σ 2 / sin ⁡ E l r s ) σ eph 2 σ ion 2 σ trop 2 σ bias 2 \begin{array}{l} \sigma^{2}F^{s} R{r}\left(a{\sigma}{ }^{2}b{\sigma}{ }^{2} / \sin E l{r}^{s}\right){\sigma{\text {eph }}}^{2}{\sigma{\text {ion }}}^{2}{\sigma{\text {trop }}}^{2}{\sigma{\text {bias }}}^{2}\end{array} σ2FsRr(aσ2bσ2/sinElrs)σeph 2σion 2σtrop 2σbias 2 其中 F s F^{s} Fs卫星系统误差因子GLONASS 1.5SBAS fact 3其它 1。 R r R{r} Rr a σ , b σ a{\sigma}, b{\sigma} aσ,bσ σ e p h \sigma{e p h} σeph σ ion \sigma{\text {ion }} σion σ trop \sigma{\text {trop }} σtrop σ bias \sigma{\text {bias }} σbias varerr() 函数只计算了第一项 static double varerr(const prcopt_t *opt, double el, int sys) {double fact,varr;factsysSYS_GLO?PPP_Glo.prcOpt_Ex.errRatioGLO:(sysSYS_CMP?PPP_Glo.prcOpt_Ex.errRatioBDS:(sysSYS_GAL?PPP_Glo.prcOpt_Ex.errRatioGAL:(sysSYS_QZS?PPP_Glo.prcOpt_Ex.errRatioQZS:opt-err[0])));varr(SQR(opt-err[1])SQR(opt-err[2])/sin(el)); // sin(el) 多了方疑似bugif (opt-ionooptIONOOPT_IF12) varrSQR(3.0); / iono-free */ //消电离层组合方差*3*3return SQR(fact)*varr; }后面的对流层、电离层、星历误差在 rescode() 中加入 var[nv]varerr(opt,azel[1i*2],sys)vare[i]vionvtrp;8、getHVRspp 四、lsqplus()最小二乘估计最小二乘准则为 V T P V min ⁡ V^{T} P V\min VTPVmin 最小二乘的一般观测模型和误差方程如下所示 { L n × 1 B n × t X t × 1 d n × 1 x t × 1 X t × 1 − X 0 , t × 1 l n × 1 L n × 1 − ( B n × t X 0 , t × 1 d n × 1 ) V n × 1 B n × 1 x t × 1 − l n × 1 , P n × n \left{\begin{array}{l} L{n \times 1}B{n \times t} X{t \times 1}d{n \times 1} \ x{t \times 1}X{t \times 1}-X{0, t \times 1} \ l{n \times 1}L{n \times 1}-\left(B{n \times t} X{0, t \times 1}d{n \times 1}\right) \ V{n \times 1}B{n \times 1} x{t \times 1}-l{n \times 1}, P{n \times n} \end{array}\right. ⎩ ⎨ ⎧Ln×1Bn×tXt×1dn×1xt×1Xt×1−X0,t×1ln×1Ln×1−(Bn×tX0,t×1dn×1)Vn×1Bn×1xt×1−ln×1,Pn×n