目录
ephclk
static int ephclk(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
double *dts)
static int ephclk(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
double *dts)
{
eph_t *eph;
geph_t *geph;
seph_t *seph;
int sys;
trace(4,"ephclk : time=%s sat=%2d\n",time_str(time,3),sat);
sys=satsys(sat,NULL);//选择系统
if (sys==SYS_GPS||sys==SYS_GAL||sys==SYS_QZS||sys==SYS_CMP||sys==SYS_IRN) {
if (!(eph=seleph(teph,sat,-1,nav))) return 0;//查找与时间最近的星历,idoe大于0时,则就是与idoe相同的星历,若idoe小于0,则选与时间最近的星历
*dts=eph2clk(time,eph);
}
else if (sys==SYS_GLO) {
if (!(geph=selgeph(teph,sat,-1,nav))) return 0;
*dts=geph2clk(time,geph);
}
else if (sys==SYS_SBS) {
if (!(seph=selseph(teph,sat,nav))) return 0;
*dts=seph2clk(time,seph);
}
else return 0;
return 1;
}
函数功能:位于satposs函数中,用来查找星历并计算卫星钟差
1.调用satsys函数来确定卫星所属系统
2.根据不同系统来调用相应seleph函数来根据teph选择与时间最接近的星历
3.调用eph2clk函数计算卫星钟差
参数分析:卫星信号发射时间,标准星历时间,卫星号,星历结构体,钟差
eph2clk
extern double eph2clk(gtime_t time, const eph_t *eph)
{
double t,ts;
int i;
trace(4,"eph2clk : time=%s sat=%2d\n",time_str(time,3),eph->sat);
t=ts=timediff(time,eph->toc);//计算起始时间
for (i=0;i<2;i++) {
t=ts-(eph->f0+eph->f1*t+eph->f2*t*t);//计算改正后的起始时间
}
return eph->f0+eph->f1*t+eph->f2*t*t;//计算钟差
}
函数功能:计算钟差
1.首先计算起始时间,当前时间与星历参考时间差值
2.通过递推公式进行迭代计算,返回计算完成的钟差
参数分析:卫星时间与星历数据
satpos
extern int satpos(const prcopt_t *popt,gtime_t time, gtime_t teph, int sat, int ephopt, const nav_t *nav, double *rs, double *dts, double *var, int *svh)
{
trace(4,"satpos : time=%s sat=%2d ephopt=%d\n",time_str(time,3),sat,ephopt);
*svh=0;
switch (ephopt) {
case EPHOPT_BRDC : return ephpos (time,teph,sat,nav,-1,rs,dts,var,svh);
case EPHOPT_SBAS : return satpos_sbas(time,teph,sat,nav, rs,dts,var,svh);
case EPHOPT_SSRAPC: return satpos_ssr (popt,time,teph,sat,nav, 0,rs,dts,var,svh);
case EPHOPT_SSRCOM: return satpos_ssr (popt,time,teph,sat,nav, 1,rs,dts,var,svh);
case EPHOPT_PREC :
if (!peph2pos(popt,time,sat,nav,1,rs,dts,var)) break; else return 1;
}
*svh=-1;//给出这个值说明卫星弃用
return 0;
}
函数功能:根据星历模式设置来选择相应的卫星坐标计算方法,若精密星历函数出计算出错,则对卫星进行标记,将svh标记为-1,弃用该颗卫星
参数分析:处理设置结构体,时间,星历参考时间,卫星号,卫星星历设置,星历结构体,卫星坐标,钟差,方差,卫星健康标志
ephpos
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],tt=1E-3;
int i,sys;
trace(4,"ephpos : time=%s sat=%2d iode=%d\n",time_str(time,3),sat,iode);
sys=satsys(sat,NULL);
*svh=-1;
if (sys==SYS_GPS||sys==SYS_GAL||sys==SYS_QZS||sys==SYS_CMP||sys==SYS_IRN) {
if (!(eph=seleph(teph,sat,iode,nav))) return 0;
eph2pos(time,eph,rs,dts,var);
time=timeadd(time,tt);
eph2pos(time,eph,rst,dtst,var);
*svh=eph->svh;
}
else if (sys==SYS_GLO) {
if (!(geph=selgeph(teph,sat,iode,nav))) return 0;
geph2pos(time,geph,rs,dts,var);
time=timeadd(time,tt);
geph2pos(time,geph,rst,dtst,var);
*svh=geph->svh;
}
else if (sys==SYS_SBS) {
if (!(seph=selseph(teph,sat,nav))) return 0;
seph2pos(time,seph,rs,dts,var);
time=timeadd(time,tt);
seph2pos(time,seph,rst,dtst,var);
*svh=seph->svh;
}
else return 0;
/* satellite velocity and clock drift by differential approx */
for (i=0;i<3;i++) rs[i+3]=(rst[i]-rs[i])/tt;
dts[1]=(dtst[0]-dts[0])/tt;
return 1;
}
函数功能:根据不同系统计算相应的卫星坐标和钟差
1.首先将svh设置为-1,根据计算结果再将新的svh赋值
2.根据系统不同来进行相应的计算,若为gps,gal,qzs,cmp,irn,则先调用seleph函数来计算卫星星历时间,再调用eph2pos来计算卫星坐标,再计算tt时间后的卫星坐标和钟差
3.和第二步相同来计算glo和sbs系统卫星坐标
4.在最后计算卫星速度和钟漂
参数分析:时间,星历参考时间,卫星号,星历结构体,卫星龄期期号,卫星坐标,钟差,方差,卫星健康标志
seleph
static eph_t *seleph(gtime_t time, int sat, int iode, const nav_t *nav)
{
double t,tmax,tmin;
int i,j=-1,sys,sel;
trace(4,"seleph : time=%s sat=%2d iode=%d\n",time_str(time,3),sat,iode);
sys=satsys(sat,NULL);
switch (sys) {
case SYS_GPS: tmax=MAXDTOE+1.0 ; sel=eph_sel[0]; break;//防止t-toe的数值正好为最大值,所以加1
case SYS_GAL: tmax=MAXDTOE_GAL ; sel=eph_sel[2]; break;
case SYS_QZS: tmax=MAXDTOE_QZS+1.0; sel=eph_sel[3]; break;
case SYS_CMP: tmax=MAXDTOE_CMP+1.0; sel=eph_sel[4]; break;
case SYS_IRN: tmax=MAXDTOE_IRN+1.0; sel=eph_sel[5]; break;
default: tmax=MAXDTOE+1.0; break;
}
tmin=tmax+1.0;
for (i=0;i<nav->n;i++) {
if (nav->eph[i].sat!=sat) continue;
if (iode>=0&&nav->eph[i].iode!=iode) continue;//卫星龄期号
if (sys==SYS_GAL) {
sel=getseleph(SYS_GAL);
if (sel==0&&!(nav->eph[i].code&(1<<9))) continue; /* I/NAV */
if (sel==1&&!(nav->eph[i].code&(1<<8))) continue; /* F/NAV */
if (timediff(nav->eph[i].toe,time)>=0.0) continue; /* AOD<=0 */
}
if ((t=fabs(timediff(nav->eph[i].toe,time)))>tmax) continue;
if (iode>=0) return nav->eph+i;
if (t<=tmin) {j=i; tmin=t;} /* toe closest to time */
}
if (iode>=0||j<0) {
trace(3,"no broadcast ephemeris: %s %s iode=%3d\n",
time_str(time,0),sat_id(sat),iode);
return NULL;
}
return nav->eph+j;
}
函数功能:在导航数据中选择星历,如果idoe>0,则选择与龄期期号相同的星历,若小于0,则选择和toe时刻最近的星历
1.获取卫星系统,根据卫星系统来获取该系统参考时间最大时间间隔
2.遍历所卫星,进行卫星号和龄期期号判断,对伽利略系统的卫星进行另外的I,F码的判断,并且要确保卫星参考时刻小于当前时间
3.确保卫星参考时间和当前时间差不超过tmax
4.若idoe>0,则直接进行赋值
5.若idoe<0,则选择最接近toe的时间
参数分析:时间,卫星号,龄期期号,星历结构体
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->A<=0.0) {
rs[0]=rs[1]=rs[2]=*dts=*var=0.0;
return;
}
tk=timediff(time,eph->toe);
switch ((sys=satsys(eph->sat,&prn))) {
case SYS_GAL: mu=MU_GAL; omge=OMGE_GAL; break;
case SYS_CMP: mu=MU_CMP; omge=OMGE_CMP; break;
default: mu=MU_GPS; omge=OMGE; break;
}
M=eph->M0+(sqrt(mu/(eph->A*eph->A*eph->A))+eph->deln)*tk;
for (n=0,E=M,Ek=0.0;fabs(E-Ek)>RTOL_KEPLER&&n<MAX_ITER_KEPLER;n++) {
Ek=E; E-=(E-eph->e*sin(E)-M)/(1.0-eph->e*cos(E));
}
if (n>=MAX_ITER_KEPLER) {
trace(2,"eph2pos: kepler iteration overflow sat=%2d\n",eph->sat);
return;
}
sinE=sin(E); cosE=cos(E);
trace(4,"kepler: sat=%2d e=%8.5f n=%2d del=%10.3e\n",eph->sat,eph->e,n,E-Ek);
u=atan2(sqrt(1.0-eph->e*eph->e)*sinE,cosE-eph->e)+eph->omg;
r=eph->A*(1.0-eph->e*cosE);
i=eph->i0+eph->idot*tk;
sin2u=sin(2.0*u); cos2u=cos(2.0*u);
u+=eph->cus*sin2u+eph->cuc*cos2u;
r+=eph->crs*sin2u+eph->crc*cos2u;
i+=eph->cis*sin2u+eph->cic*cos2u;
x=r*cos(u); y=r*sin(u); cosi=cos(i);
/* beidou geo satellite */
if (sys==SYS_CMP&&(prn<=5||prn>=59)) { /* ref [9] table 4-1 */
O=eph->OMG0+eph->OMGd*tk-omge*eph->toes;
sinO=sin(O); cosO=cos(O);
xg=x*cosO-y*cosi*sinO;
yg=x*sinO+y*cosi*cosO;
zg=y*sin(i);
sino=sin(omge*tk); coso=cos(omge*tk);
rs[0]= xg*coso+yg*sino*COS_5+zg*sino*SIN_5;
rs[1]=-xg*sino+yg*coso*COS_5+zg*coso*SIN_5;
rs[2]=-yg*SIN_5+zg*COS_5;
}
else {
O=eph->OMG0+(eph->OMGd-omge)*tk-omge*eph->toes;
sinO=sin(O); cosO=cos(O);
rs[0]=x*cosO-y*cosi*sinO;
rs[1]=x*sinO+y*cosi*cosO;
rs[2]=y*sin(i);
}
tk=timediff(time,eph->toc);
*dts=eph->f0+eph->f1*tk+eph->f2*tk*tk;
/* relativity correction */
*dts-=2.0*sqrt(mu*eph->A)*eph->e*sinE/SQR(CLIGHT);
/* position and clock error variance */
*var=var_uraeph(sys,eph->sva);
}
函数功能:计算卫星坐标和钟差,方差
1.定义计算过程所需要的一系列参数
2.按照计算平近点角,偏近点角,真近点角等公式来计算卫星坐标
3.对钟差进行相对论改正
4.根据ura参数计算坐标和钟差的方差
参数分析:时间,星历结构体,坐标,钟差,方差
rescode
static int rescode(int iep,int iter, 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, const ssat_t *ssat, double *v, double *H, double *var, double *azel, int *vsat, double *resp, int *ns,double *ztrp, const kf_t *ins_kf,int sdopt,double *Ri,double*Rj,int *nb,int *b,int *vflg,int *exc)
函数功能:进行计算最小二乘所需要的设计矩阵,残差阵及方差阵等等
1.根据是否进行星间单差来选择不同的模式
2.在这里写未选择星间单差
3.遍历所有观测值,将卫星可用状况,卫星高度角,伪距残差设置为0,时间和卫星号则配置观测值结构体中相应的数值
4.检查是否有重复的观测值数据,如果有,则进行标记并重新循环
5.调用satexclude函数进行排除有问题的卫星
6.调用geodist函数进行计算几何距离和视向量
7.调用satazel函数计算高度角
8.调用testsnr函数进行信噪比检测
9.调用ionocorr函数进行电离层模型改正
10.将计算好的电离层和方差根据频率进行相应改正
11.调用tropcorr函数计算对流层延迟
12.调用prange函数进行伪距计算
13.计算残差阵
14.计算设计矩阵,将xyz系数赋值
15.调用varerr函数来计算方差阵
16.为防止秩亏增加观测方程
参数分析:iep(未理解),迭代数,观测值结构体,观测值个数,卫星坐标,卫星钟差,卫星方差,卫星健康状况,星历结构体,状态矩阵,参数设置,卫星结构体,残差阵,系数矩阵,方差阵,高度角,卫星可用标志,伪距残差,卫星数目,对流层误差,有关惯导参数,单差设置,星间单差时的卫星1方差,星间单差时的卫星2方差,后面两个都是在星间单差时的设置参数,卫星健康标志,卫星排除标志
lsq
extern int lsq(const double *A, const double *y, int n, int m, double *x, double *Q)
{
double *Ay;
int info;
if (m<n) return -1;
Ay=mat(n,1);
matmul("NN",n,1,m,1.0,A,y,0.0,Ay); /* Ay=A*y */
matmul("NT",n,n,m,1.0,A,A,0.0,Q); /* Q=A*A' */
if (!(info=matinv(Q,n))) matmul("NN",n,1,n,1.0,Q,Ay,0.0,x); /* x=Q^-1*Ay */
free(Ay);
return info;
}
函数功能:最小二乘计算,用于计算接收机位置钟差等状态参数
1.公式如图
2.此处权阵已在上一步进行配权,并且在该程序中转置阵和原阵是相反的,并且矩阵按列存储,先计算这一部分,再计算
最后进行相乘并求逆
参数分析:设计矩阵即系数阵,残差阵,待估参数个数和观测方程个数,状态矩阵,协方差阵。
valsol
static int valsol(int iep,gtime_t t,const double *azel, const int *vsat, int n, const prcopt_t *opt, const double *v, int nv, int nx, char *msg,double *dop)
{
double azels[MAXOBS*2],vv;
int i,ns;
trace(4,"valsol : n=%d nv=%d\n",n,nv);
/* chi-square validation of residuals */
vv=dot(v,v,nv);
if (nv>nx&&vv>chisqr[nv-nx-1]) {
trace(2,"%s(%d) large chi-square error nv=%d vv=%.1f thres=%.1f in SPP\n",time_str(t,1),iep,nv,vv,chisqr[nv-nx-1]);
/* return 0; */ /* threshold too strict for all use cases, report error but continue on */
}
/* large gdop check */
for (i=ns=0;i<n;i++) {
if (!vsat[i]) continue;
azels[ ns*2]=azel[ i*2];
azels[1+ns*2]=azel[1+i*2];
ns++;
}
dops(ns,azels,opt->elmin,dop);
if (dop[0]<=0.0||dop[0]>opt->maxgdop) {
sprintf(msg,"gdop error nv=%d gdop=%.1f",nv,dop[0]);
return 0;
}
return 1;
}
函数功能:进行卡方检验和gdop检验
1.计算残差阵内积,通过检测内积是否大于卡方要求数值
2.将高度角矩阵进行复制,只复制那些标记有用的卫星
3.检测gdop,若小于0或大于最大容差,则报错并返回0
参数分析:iep(未理解),高度角,可用卫星标志,卫星个数,参数设置,残差阵,方程个数,待估参数个数,错误信息,dop值