singlep.c

GPS详尽的代码

singlep.c

void PC12(Pc1,Pc2,Pd1,Pd2)
/* 函数 PC12：根据 C1,C2,P1,P2,Frequency_n 决定 Pc1,Pc2,pd1,pd2（观测量编码的列号）*/
int *Pc1,*Pc2,*Pd1,*Pd2;
{    int pc1,pc2,pd1,pd2;
     if(Code==0) 
      { if(P1<100) pc1=P1; 
        else
         { if(C1<100) pc1=C1; 
           else 
             { if(P2<100) pc1=P2; else pc1=C2; }}}
     else 
       {if(Code==1) pc1=P1; if(Code==2) pc1=P2;
        if(Code==3) pc1=C1; if(Code==4) pc1=C2;} 
     if(pc1==100) pc1=P1; if(pc1==100) pc1=C1;
     if(Frequency_n==2) { pc2=P2; if(C2<100) pc2=C2; if(pc2==100) pc2=pc1; }
     else pc2=pc1;
     *Pc1=pc1; *Pc2=pc2;
     pd1=D1; pd2=D2; 
     if(pd1==100&&pd2==100) printf("without Doppler data\n");
     else {if(pd1==100) pd1=pd2; else pd2=pd1;}
     *Pd1=pd1; *Pd2=pd2;}
double twoi2f(ih,im)
/* 应用f = ih + (im), where (im)<1.将函数 twoi2f 变成两个浮点数 */ 
int ih,im;
{  double f;
   f=ih;
   if(im<10) {f+=im/10.; return(f);}
   if(im>=10 && im<=99) {f+=im/100.; return(f);}
   if(im>=100 && im<=999) {f+=im/1000.; return(f);}
   if(im>=1000 && im<=9999) {f+=im/10000.; return(f);}
   if(im>=10000 && im<=99999) {f+=im/100000.; return(f);}}
void singlep(icon,x0,Height,xhidat,odat_t,odat_dt,odat_n,id_odat,iodat,odat,
borb,itborb,Tr1,singlex,singlet) 
int icon[I10*Isat];
double x0[3*Ista],Height[Ista];
long xhidat[2*Isat*Ista];
double odat[Isato*10*Ista*Iepoches],odat_t[Iepoches],odat_dt[Ista*Iepoches];
long odat_n[Iepoches],id_odat[I10*Ista*Iepoches];
unsigned int iodat[Isato*10*Ista*Iepoches];
long itborb[8]; /* orbit */
double borb[Isat*I96*8];
double Tr1[I3*Ista*I96]; /* tide */
double singlex[3*Ista*Iepoches],singlet[Ista*Iepoches];
/*函数 singlep：应用原始伪距数据组成观测方程，求解地心坐标和钟差*/  
{    int i,i1,j,j1,j2,k,k1,k5,I1,I2,J1,J2,K=0,K1=0,hour,minute;
     double T=20.,P=1013.,rh=0.5,sum,sumc,sec,ff;
     double zdis,z4,f4,height;
     double xi1,yi1,zi1,xj1,yj1,zj1,txi1,tyi1,tzi1,xj2,yj2,zj2;
     double xj1dot,yj1dot,zj1dot,dcj1,dcj2,dcs;
     double orbj1[8],pj[6],pi[3],Tr[3*Ista],dd_rela,dd_trop,dd_cloc;
     double dt,dt1,dti4,dis;
     double am[10*4],lo[10],an[Nlow],bn[Iunknown],accur[Iunknown];
     double singlx[3*Ista],singlt[Ista],tau,ltpl;
/* 应用PC12()决定 PC1 和 PC2 with; */
/* 为控制输出数据 */
     if(singlx[0]<=0.) for(j2=0;j2<3*icon[0];j2++) singlx[j2]=x0[j2];
     for(k=0;k<Iepoches;k++) {   /* k历元 */
/* 设 singlx[]=x0[] 并初始化 */
        for(j2=0;j2<icon[0];j2++) singlet[j2+k*Ista]=0.; 
        for(j2=0;j2<icon[0];j2++) singlt[j2]=0.;
/* 求时间 */
        t_hms(odat_t[k],&hour,&minute,&sec); 
        for(j2=0;j2<icon[0];j2++) {  /* 测站 */
/* 迭代 */ dt1=0.;
           K=0; iteration1:; K+=1;
           dt=odat_dt[j2+k*Ista];
           i=0; /* 观测数累计 i */
           for (k1=0;k1<10;k1++) lo[k1] = 0.;
           for (k1=0;k1<4*10;k1++) am[k1] = 0.;
           for(j1=0;j1<odat_n[k];j1++) {  /* 数据数 j1 */
/* 求出 id_odat[] idex I1,J1 */
              I1=id_odat[j1+I10*Ista*k]/100; 
              J1=id_odat[j1+I10*Ista*k]-100*I1;
              if(I1==j2+1) { /* only for one station I1 */
/* 求测站坐标. I1, 卫星. J1，卫星钟改正数. J1 (单位： s/1000000) */
                 xi1=singlx[0+(I1-1)*3]; yi1=singlx[1+(I1-1)*3];
                 zi1=singlx[2+(I1-1)*3];
                 dti4=odat_t[k]/*+dt*/;
                 k5=getorb(itborb,borb,J1,dti4,icon[25],orbj1);/*sat. J1*/
                 if(k5==0) {printf("no sat_%d data available\n",J1); exit(0);}
                 xj1=orbj1[0]; yj1=orbj1[1]; zj1=orbj1[2]; 
                 dcj1=orbj1[6]; dcs = dcj1*C*1.e-6;
/* 卫星速度 */
                 xj1dot=orbj1[3]; yj1dot=orbj1[4]; zj1dot=orbj1[5];
                 f4 = 0.;
                 z4= 0.;
/* 发送时间和地球自转改正 */
                 xj2=xj1; yj2=yj1; zj2=zj1;
                 transrot(&xj1,&yj1,&zj1,xj1dot,yj1dot,zj1dot,
                          xi1,yi1,zi1,&dis,&tau);
/* 计算的观测值. */
                 sumc = dis; /* p0j1i1; dist. component */
                 sumc += f4; /* tropospheric effect */
                 sumc += z4; /* relative effect */
                 sumc -= dcs; /* satellite clock effect */
                 sumc += singlet[j2+k*Ista]*C*1.e-6; 
/* 接收机钟差 singlet （ 1.e+6 秒）， sumc 单位：米 */
/* 求出伪距观测量，并组成无电离层的组合 */
                 k5=0;
                 if(iodat[PC1+j1*Isato+Isato*I10*Ista*k]==1) k5=1;
                 sum=odat[PC1+j1*Isato+Isato*I10*Ista*k]; 
                 ff = (FL1*FL1-FL2*FL2);
                 sum=sum*(FL1*FL1)/ff;
                 sum=sum-odat[PC2+j1*Isato+Isato*I10*Ista*k]*(FL2*FL2)/ff;
                 sum+=dt*C;
                 if(k5==0) goto nextj1; else i+=1; 
/* 对每个测站 i1,： 3 个坐标未知数 + 1 钟未知数 */
/* 组成观测方程 */
                 txi1 = -(xj1-xi1)/dis;
                 tyi1 = -(yj1-yi1)/dis;
                 tzi1 = -(zj1-zi1)/dis;
                 am[0+(i-1)*4] = txi1;
                 am[1+(i-1)*4] = tyi1;
                 am[2+(i-1)*4] = tzi1;
                 am[3+(i-1)*4] = 1.;
                 lo[i-1]=sum-sumc;
                 nextj1:; /* 下一个观测数据 */
              } /* if I1==j2+1 */
           } /* 数据数 j1 */

/* 为控制输出观测方程 */
/* 组成法方程式 */
           for (i1=0;i1<4;i1++) { /* 计算 an[][] */
              for (j1=0;j1<=i1;j1++) {
                 sum = 0.; 
                 for (k1=0;k1<i;k1++) {
                    sum += am[i1+k1*4]*am[j1+k1*4];}
                 an[j1+i1*(i1+1)/2] = sum; }
              sum = 0.; /* calculate bn[] */
              for (k1=0;k1<i;k1++) {
                 sum += am[i1+k1*4]*lo[k1];}
              bn[i1] = sum; } 
           ltpl=0.; for(j1=0;j1<=i;j1++) ltpl+=lo[j1]*lo[j1];
/* 输出法方程式 */
/* 检查和求解 */
           if(i<4) {printf("Obs_n < 4, singlep not possible\n"); 
               printf("t = %6.1f sta_n = %d \n",odat_t[k],j2);
               singlt[j2]=odat_dt[j2+k*Ista];
               goto sss;}
           if(j2>=icon[0]-Kstation)       /* 静态测站 */
            { sum=0.; sumc=0.; 
              for(j1=0;j1<i;j1++) {sum+=lo[j1]/i; sumc+=lo[j1]*lo[j1]/i;}
              singlt[j2]=(sum/C)*1.e+6;
              sumc-=sum*sum; sumc=fabs(sumc); sumc=sqrt(sumc);}
            else						/* 动态测站 */
             {sum = ls(stdout,an,bn,accur,ltpl,4,i);
              for (j=0;j<3;j++) if(fabs(bn[j])>0.1)K1+=1;
              singlt[j2]=(bn[3]/C)*1.e+6;
              dt1+=singlt[j2]*1.e-6;
              singlx[0+j2*3]+=bn[0]; singlx[1+j2*3]+=bn[1];
              singlx[2+j2*3]+=bn[2]; }
           sss:;
/* 接收机钟差 */
           singlet[j2+k*Ista]+=singlt[j2];
/* 迭代 */
           if(K1!=0) {K1=0; 
              if(K>=6) printf("iteration number = 6\n"); 
              else goto iteration1;}
/* 单点定位坐标结果 */
           for(j=0;j<3;j++) singlex[j+j2*3+k*icon[0]*3]=singlx[j+j2*3];
/* 输出结果 */
/* 如果接收机钟差为正值（+），则观测伪距应 - C*singlet */
        } /* 测站 j2 */
     } /* 历元 k */
}
void transrot1(xs,ys,zs,xsv,ysv,zsv,xr,yr,zr,ddis,dtau)
double *xs,*ys,*zs,*xsv,*ysv,*zsv,xr,yr,zr,*dtau,*ddis;
/* 发送时间和地球自转效应函数 */ 
{      double tau,tau1,tau2,d,xs1,ys1,zs1,xsv1,ysv1,zsv1;
       xs1=*xs; ys1=*ys; zs1=*zs; xsv1=*xsv; ysv1=*ysv; zsv1=*zsv;
/*  发送时间和地球自转 */ 
       tau1=0.; again2:;
       d=distance(xs1,ys1,zs1,xr,yr,zr);
       tau=d/C;
       if(Iearthrot>=1) 
        { tau2=tau-tau1;
          earthrot(tau2,&xs1,&ys1,&zs1);
          if(Iearthrot==2) earthrot(tau2,&xsv1,&ysv1,&zsv1);
          d=distance(xs1,ys1,zs1,xr,yr,zr);
          tau=d/C;}
       xs1=xs1-(tau-tau1)*xsv1;ys1=ys1-(tau-tau1)*ysv1;zs1=zs1-(tau-tau1)*zsv1;
       if(fabs(tau-tau1)<=0.0000001) { ; 
          d=distance(xs1,ys1,zs1,xr,yr,zr);
          tau=d/C;
          if(Iearthrot>=1) 
           { tau2=tau-tau1;
             earthrot(tau2,&xs1,&ys1,&zs1);
             if(Iearthrot==2) earthrot(tau2,&xsv1,&ysv1,&zsv1);
             d=distance(xs1,ys1,zs1,xr,yr,zr);
             tau=d/C;}}
       else {tau1=tau; goto again2; }
       *ddis = d; *dtau = tau;
       *xs=xs1; *ys=ys1; *zs=zs1;
       *xsv=xsv1; *ysv=ysv1; *zsv=zsv1; }
void transrot(xs,ys,zs,xsv,ysv,zsv,xr,yr,zr,ddis,dtau)
double *xs,*ys,*zs,xsv,ysv,zsv,xr,yr,zr,*dtau,*ddis;
/*  发送时间和地球自转效应计算函数 */ 
{      double tau,tau1,tau2,d,xs1,ys1,zs1,xsv1,ysv1,zsv1;
       xs1=*xs; ys1=*ys; zs1=*zs; xsv1=xsv; ysv1=ysv; zsv1=zsv;
/*  发送时间和地球自转 */ 
       tau1=0.; again2:;
       d=distance(xs1,ys1,zs1,xr,yr,zr);
       tau=d/C;
       if(Iearthrot>=1) {
          tau2=tau-tau1;
          earthrot(tau2,&xs1,&ys1,&zs1);
          if(Iearthrot==2) earthrot(tau2,&xsv1,&ysv1,&zsv1);
          d=distance(xs1,ys1,zs1,xr,yr,zr);
          tau=d/C; }
       xs1=xs1-(tau-tau1)*xsv1;ys1=ys1-(tau-tau1)*ysv1;zs1=zs1-(tau-tau1)*zsv1;
       if(fabs(tau-tau1)<=0.0000001) { ; 
          d=distance(xs1,ys1,zs1,xr,yr,zr);
          tau=d/C;
          if(Iearthrot>=1) {
             tau2=tau-tau1;
             earthrot(tau2,&xs1,&ys1,&zs1);
             if(Iearthrot==2) earthrot(tau2,&xsv1,&ysv1,&zsv1);
             d=distance(xs1,ys1,zs1,xr,yr,zr);
             tau=d/C;}}
       else {tau1=tau; goto again2; }
       *ddis = d; *dtau = tau;
       *xs=xs1; *ys=ys1; *zs=zs1;}
double distance(x1,y1,z1,x2,y2,z2)
/* 计算两点的距离 */
 double x1,y1,z1,x2,y2,z2;
{  double dis;
   dis = (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2) + (z1-z2)*(z1-z2);
   dis = sqrt(dis); return(dis);}
void earthrot(tau,x,y,z)
/* 函数 earthrot 应用发送时间 tau 计算地球自转改正 */
double tau,*x,*y,*z;
{    int i,j,k,k1;
     double R[3][3],Xx[3],v[3],cl,sl,omiga,alpha,sum;
/* 旋转矩阵 R初始化 */
     omiga = 0.72921151467e-4;
     alpha = omiga*tau;
alpha*=Iluisa;
     cl = cos(alpha); sl = sin(alpha);
     R[0][0] = cl;     R[0][1] = sl;     R[0][2] = 0.;
     R[1][0] = -sl;    R[1][1] = cl;     R[1][2] = 0.;
     R[2][0] = 0.;     R[2][1] = 0.;     R[2][2] = 1.;
     Xx[0]=*x; Xx[1]=*y; Xx[2]=*z; 
/* 向量旋转 */
     for (i=0;i<3;i++) { v[i] = 0.;
        for (j=0;j<3;j++) {
           v[i] += R[i][j]*Xx[j]; }}
     *x = v[0]; *y = v[1]; *z = v[2];
/* 为控制的输出 */
}