eqdd_s.c

GPS详尽的代码

        txi1 = xj1 + xj1i1 - xi1 ;
        tyi1 = yj1 + yj1i1 - yi1 ;
        tzi1 = zj1 + zj1i1 - zi1 ;
        p0j1i1 = sqrt(txi1*txi1 + tyi1*tyi1 + tzi1*tzi1);
        /* iteration is not necessary, accuracy 10**-6 would enough */
        /* check the clock error correction */
        /* form obs. equations which depend on .... combinations form,
         obs. type etc. */
        /* calculated obs. */
        sumc = p0j2i2 - p0j2i1 - p0j1i2 + p0j1i1; /* dist. component */
        if(icon[30]==1) sumc += dd_trop; /* tropospheric effects */
        if(O_eq<=1)printf("sumc=%f ",sumc);
        if(icon[33]==1) sumc += dd_rela; /* relative effects */
        if(O_eq<=1)printf("sumc+dd_rela=%f \n",sumc);
        if(icon[31]>=1) sumc += dd_cloc; /* clock errors */
        /* sumc unit : meter */
        if(icon[35]<10) { /* Super parameter<10, only 1 type obs. used */
           if(icon[20]<=5) sumc = sumc/Lambdafc; /*change unit: cycle */
           k5=0;
           switch(icon[20]) { /* single type dd obs. used */
           case 0: { if(iddat[L1+j1*Isato+Isato*I10*Ista*k]==1) k5=1;
                   sum=ddat[L1+j1*Isato+Isato*I10*Ista*k]; break;}
           case 1: { if(iddat[L2+j1*Isato+Isato*I10*Ista*k]==1) k5=1;
                   sum=ddat[L2+j1*Isato+Isato*I10*Ista*k]; break;}
           case 2: { if(iddat[L1+j1*Isato+Isato*I10*Ista*k]==1 &&
                     iddat[L2+j1*Isato+Isato*I10*Ista*k]==1) k5=1;
                   sum= ddat[L1+j1*Isato+Isato*I10*Ista*k] -
                   ddat[L2+j1*Isato+Isato*I10*Ista*k]*(FL2/FL1); break;}
           case 3: { if(iddat[L1+j1*Isato+Isato*I10*Ista*k]==1 &&
                     iddat[L2+j1*Isato+Isato*I10*Ista*k]==1) k5=1;
                   sum= ddat[L1+j1*Isato+Isato*I10*Ista*k] -
                   ddat[L2+j1*Isato+Isato*I10*Ista*k]*(FL2/FL1); 
                   sum = sum*0.77; break;}
           case 4: { if(iddat[L1+j1*Isato+Isato*I10*Ista*k]==1 &&
                     iddat[L2+j1*Isato+Isato*I10*Ista*k]==1) k5=1;
                   sum= ddat[L1+j1*Isato+Isato*I10*Ista*k] -
                   ddat[L2+j1*Isato+Isato*I10*Ista*k]; break;}
           case 5: { if(iddat[L1+j1*Isato+Isato*I10*Ista*k]==1 &&
                     iddat[L2+j1*Isato+Isato*I10*Ista*k]==1) k5=1;
                   sum=icon[21]*ddat[L1+j1*Isato+Isato*I10*Ista*k]/icon[22] 
                   +icon[23]*ddat[L2+j1*Isato+Isato*I10*Ista*k]/icon[24];
                   break;}
           case 6: { if(Code==0) {if(P1<100) PC=P1; else {
                        if(P2<100) PC=P2; else {
                           if(C1<100) PC=C1; else PC=C2;
                     }}} else {
                        if(Code==1) PC=P1; if(Code==2) PC=P2;
                        if(Code==3) PC=C1; if(Code==4) PC=C2;
                     }
                     /* ion-free combination */
                     if(iddat[PC+j1*Isato+Isato*I10*Ista*k]==1) k5=1;
                   sum=ddat[PC+j1*Isato+Isato*I10*Ista*k]; 
                   ff = (FL1*FL1-FL2*FL2);
                   sum=sum*(FL1*FL1)/ff;
                   sum=sum-ddat[PC2+j1*Isato+Isato*I10*Ista*k]*(FL2*FL2)/ff;
                   break;}
           case 7: { if(D1<100) PC=D1; else PC=D2;
                     if(iddat[PC+j1*Isato+Isato*I10*Ista*k]==1) k5=1;
                   sum=ddat[PC+j1*Isato+Isato*I10*Ista*k]; break;}
           }
           if(k5==0) goto nextj1; 
           /* for baseline components :
           pji2=sqrt((xj-xi2)**2+(yj-yi2)**2+(zj-zi2)**2)
           pji2=p0ji2+(dpji2/dxi2)*dxi2+(dpji2/dyi2)*dyi2+(dpji2/dzi2)*dzi2
           dpji2/dxi2 = -(xj-xi2)/pji2
           dpji2/dyi2 = -(yj-yi2)/pji2
           dpji2/dzi2 = -(zj-zi2)/pji2
           for reference station dxi1 = dyi1 = dzi1 = 0 */
           /* for every baseline, i.e. station i1, 3 unknowns 
           dd = od(i2,j2) - od(i1,j2) - od(i2,j1) + od(i1,j1) */
           txi2 = (xj2+xj2i2-xi2)/p0j2i2 - (xj1+xj1i2-xi2)/p0j1i2;
           tyi2 = (yj2+yj2i2-yi2)/p0j2i2 - (yj1+yj1i2-yi2)/p0j1i2;
           tzi2 = (zj2+zj2i2-zi2)/p0j2i2 - (zj1+zj1i2-zi2)/p0j1i2;
           txi2 = -txi2; tyi2 = -tyi2; tzi2 = -tzi2;
           if(icon[20]<=5) {txi2/=Lambdafc;tyi2/=Lambdafc;tzi2/=Lambdafc;}
           am[0+(I2-2)*3+K*Iunknown] = txi2;
           am[1+(I2-2)*3+K*Iunknown] = tyi2;
           am[2+(I2-2)*3+K*Iunknown] = tzi2;
           /* tropospheric parameter (canceled here) */
           /* denote the station coord. unknowns id, initial values, time */
           if(icon[201]==0) { 
              for(k2=1;k2<icon[0];k2++) { k3=(k2-1)*3;
                 N0[k3]=0; N0[k3+1]=0; N0[k3+2]=0;
                 /* N0t is defined as (long)(odat_t[]*10) */
                 N0t[k3]=(long)(odat_t[k]*10);    N0te[k3]=N0t[k3];
                 N0t[k3+1]=(long)(odat_t[k]*10);  N0te[k3+1]=N0t[k3+1];
                 N0t[k3+2]=(long)(odat_t[k]*10);  N0te[k3+2]=N0t[k3+2];
                 Fi0[k3]=0.; Fi0[k3+1]=0.; Fi0[k3+2]=0.;
                 /* id of coord. <x,y,z> = station_n*10+<1,2,3> */
                 idN0[k3]=(k2+1)*10+1; idN0[k3+1]=(k2+1)*10+2; 
                 idN0[k3+2]=(k2+1)*10+3;
              }
              icon[201] = 3*(icon[0]-1);
              /* troposphere parameter as unknown, every i one parameter
              (canceled here) */
           }
           if(icon[20]<=5) {
              /* for k-th obs. there is a k-th ambiguity parameter
              unit : cycle, wavelength Lambdafc */
              /* according to id_slips[], slips_t[] to decide 
              if this ambiguity parameter is a new unknown */
              for (j2=0,k1=0;k1<Islip;k1++) {
                 if(fabs(odat_t[k]-slips_t[k1])<0.2){/*there is slips at time*/
k2=id_slips[k1]/10000; if(k2==I1||k2==I2) {
                    k2=id_slips[k1]/100-100*(id_slips[k1]/10000);
                    if(k2==J1||k2==J2) {/* slips for this obs.*/
                       k2=id_slips[k1]-100*(id_slips[k1]/100);
                       if(k2==3 || k2==0) j2=1; /* cycle slips or new obs. */
                       if(icon[20]==0 && k2==1) j2=1;
                       if(icon[20]==1 && k2==2) j2=1;
                       if(icon[20]>=2) j2=1;
                       /*if fulfill four condi.,then slips*/
}              }}}
              if(j2==1) {
              /*new obs or cycle slips, if next epoch has a cycle slips
              with same id, then go over this obs*/
                 for (k1=0;k1<Islip;k1++) {
                    if(fabs(odat_t[k+1]-slips_t[k1])<0.2) {
                    /* there is a cycle slips at the next epoch*/   
 k2=id_slips[k1]/10000; if(k2==I1||k2==I2) {
                      k2=id_slips[k1]/100-100*(id_slips[k1]/10000);
                       if(k2==J1||k2==J2) {/* slips for this obs.*/
                          k2=id_slips[k1]-100*(id_slips[k1]/100);
                          if(k2!=10) goto nextj1; 
                          /*next epoch there is a cycle slips for this
                          id of obs, therefore go over this obs.*/
}              }}}}
              /* initial int. vector N0,idN0,Fi0,N0t */
              if(j2 == 1) { /* this is a new unknown */
                 k4 = icon[201];
                 am[k4+K*Iunknown] = 1.; 
                 Fi0[k4] = (long)sum;  /* Fi0 is initial obs. int. part */ 
                 if(sum<0.) Fi0[k4]-=1; /*Fi0[k4]=0;*/
                 idN0[k4] = id_ddat[j1+I10*Ista*k];
                 sum -= Fi0[k4];       /* eliminate the initial part */
                 N0[k4] = (long)sumc; /* N0 is int. part of sumc */
                 if(sumc<0.) N0[k4]-=1; /*N0[k4]=0;*/
                 lo[K] = sum - (sumc - N0[k4]);    /* obs. vector */
                 /* the initial ambiguity is (-N0[]+Fi0[]) 
                 (should be added to obs. if necessary) */
                 N0t[k4] = (long)(odat_t[k]*10);/* set N0t */
                 N0te[k4]=N0t[k4];
                 if(Run_n==4) { k2=-1;
                    for(k1=0;k1<Nfix;k1++) {
                       if(idlo[K]==N0xid[k1]) {
                          if(N0xtb[k1]<=N0t[k4] && N0t[k4]<=N0xte[k1]-1) k2=k1;
                    }}
                    if(k2==-1) {printf("N not found: error !!!!!!!\n");
                    goto nextj1;} 
                 } lo[K] -= N00f[k2];
                 icon[201] += 1; N00_n[k4]+=1;
              } else { 
                        /* This is not a new unknown,
                           N0,Fi0 have to be found (backward) and used.
                           if can not be found, error */
                 for(k5=0,k1=icon[201]-1;k1>=0;k1--) {
                    if(id_ddat[j1+I10*Ista*k]==idN0[k1]) { k5 = 1;
                       sum -= Fi0[k1];    /* eliminate the initial part */
                       lo[K] = sum - (sumc - N0[k1]);  /* obs. vector */
                       k4 = k1;
                       am[k4+K*Iunknown] = 1.; 
                       N0te[k4]=(long)(odat_t[k]*10);/* set N0te */;
                       N00_n[k4]+=1;
                       if(Run_n==4) { k2=-1;
                          for(k1=0;k1<Nfix;k1++) {
                          if(idlo[K]==N0xid[k1]) {
                          if(N0xtb[k1]<=(long)(odat_t[k]*10) &&
                          (long)(odat_t[k]*10)<=N0xte[k1]-1) k2=k1;
                          }}
                          if(k2==-1) {printf("N not found: error !!!O!!!\n");
                          goto nextj1;}
                          lo[K] -= N00f[k2]; 
                       } 
                    }
                    if(k5==1)goto loopoe;/*if found, end the find process*/
                 }
                 if(k5==0) { /*ambiguity is neither new nor old one*/
                    /*used for delete the first new obs*/
                    goto nextj1;
                 }
                 loopoe:;
              }
           } /* end of if(icon[20]<=5) */
           else {
              if(icon[20]==6) lo[K]=sum-sumc;
           }
           K+=1; icon[200]=K;
           nextj1:;
        } /* end of if(icon[35]==1) */
     } /* end of for(j1=.....) */
/* Output for control */
}