jave-ppp.txt

精密单点定位程序

//*** compute ecbf coordinates

      xk = xpk*comgk-ypk*somgk*ceyek;
      yk = xpk*somgk+ypk*comgk*ceyek;
      zk =           ypk      *Math.sin(eyek);

      return new PosT(xk, yk, zk, bcclok, dtr);
   }

   public double eccano(double t) {

//*** compute eccentric anomaly at time T from broadcast elements

      double a,en0,tk,en,emk;

      if( (t-t0e) >  302400.0 ) t=t-604800.0;      //*** handle week rollover
      if( (t-t0e) < -302400.0 ) t=t+604800.0;

      a=sqrta*sqrta;               //*** semimajor axis
      en0=Math.sqrt(gm/(a*a*a));   //*** mean motion
      tk=t-t0e;                    //*** time since orbit reference epoch
      en=en0+deln;                 //*** corrected mean motion
      emk=em0+en*tk;               //*** mean anomaly, M
      return kepEQ(emk);           //*** solve kepler's eq for ecc anomaly, E
   }

   public double kepEQ(double em) {
      
//*** solve for eccentric anomaly given mean anomaly and orbital eccentricity
//*** use simple fixed point iteration of kepler's equation

      double ecca, ecca0;           //*** iterates of eccentric anomaly
      
//*** initialize eccentric anomaly

      ecca=em+e*Math.sin(em);
      
//*** exit only on convergence

      do {
         ecca0=ecca;
         ecca=em+e*Math.sin(ecca0);
      }  while (Math.abs( (ecca-ecca0)/ecca ) > 1.e-14);
      return ecca;
   }

   public void BCinit() {

//*** initialize to test values

      t0c   =  3.10400000000000e+03;
      af2   =  0.00000000000000e+00;
      af1   =  2.04636307898909e-12;
      af0   =  4.73577529191971e-07;

      crs   = -1.74687500000000e+01;
      deln  =  4.57340478638085e-09;
      em0   =  1.63307323511810e+00;

      cuc   = -9.68575477600099e-07;
      e     =  4.74695244338363e-03;
      cus   =  2.30222940444947e-06;
      sqrta =  5.15357911109924e+03;

      t0e   =  3.10400000000000e+03;
      cic   =  7.26431608200074e-08;
      omg0  =  1.12194333211744e+00;
      cis   = -4.28408384323121e-08;

      eye0  =  9.59844678252147e-01;
      crc   =  3.32750000000000e+02;
      w     =  1.44611981813501e+00;
      omgdot= -8.27498754358907e-09;

      eyedot= -7.96461747257267e-11;

      return;
   }

//*** expected test results:

//***  myBC.BCinit();
//***  myPos = myBC.bcxyzt( 3104.0);
//***  System.out.println(myPos.toString4());
//***  myPos = myBC.bcxyzt( 3204.0);
//***  System.out.println(myPos.toString4());
//***  myPos = myBC.bcxyzt( 4104.0);
//***  System.out.println(myPos.toString4());
//***  myPos = myBC.bcxyzt(13104.0);
//***  System.out.println(myPos.toString4());

//***  x=-17212647.7969  y=-20205593.2676  z=  1151304.1641
//***  x=-17199667.6706  y=-20234605.2305  z=   834485.2910
//***  x=-16972873.7180  y=-20363862.4894  z= -2016782.8591
//***  x= -2061865.9599  y=-15633481.5430  z=-21527450.2128

}//***endclass BCstuf
//****************************************************************************
class SP3set {

//*** collection of precise orbits object

   private static final int    MXPRN = 32;         //*** 38 are possible
   private static final int    MXTRW = 3*4*24+1;   //*** time rows (3 days)
   private static final double TINY  = 1.0e-2;     //*** tiny number
   private static final double EPS   = 1.0e-13;    //*** very tiny number
   private static final double BADT  = 0.999999;   //*** bad time flag

   private Gtime  tStuf;                   //*** time object for SP3Set
   private int    nsat=0;                  //*** sats in input file
   private int    nep=0;                   //*** number of time epochs

   private double tstr = 0;                //*** time of 1 st epoch
   private double tstp = 0;                //*** time of last epoch
   private double dlt  = 0;                //*** epoch interval
   private int    imx  = 0;                //*** max index into arrays
   private double t0mx = 0.0;              //*** time for max index

   private double c[]     = new double [9+1];              //*** interp coeff.
   private double xs [][] = new double [MXTRW+1][MXPRN+1]; //*** fortran style
   private double ys [][] = new double [MXTRW+1][MXPRN+1]; 
   private double zs [][] = new double [MXTRW+1][MXPRN+1]; 
   private double ts [][] = new double [MXTRW+1][MXPRN+1];

   private int    ierr;                    //*** error code
   private double xSP3;                    //*** x of sat
   private double ySP3;                    //*** y of sat
   private double zSP3;                    //*** d of sat
   private double tSP3;                    //*** dt of sat

//***
//*** compute methods
//***

   public boolean oterp(int iprn, double tsec){

//*** interpolate sp3 orbit
//*** iprn is prn identifier for a satellite
//*** input time, tsec, is gps seconds from ref. epoch  (see Gtime)

      int    i,intt,ioff;
      double r,t,t0,tint, powr, dt, fac;

//*** satellite prn number not loaded from the sp3 file (1st epoch 0 check)

      r=xs[1][iprn]*xs[1][iprn]+ys[1][iprn]*ys[1][iprn]+zs[1][iprn]*zs[1][iprn];
      if(r <= TINY){
         ierr=1;
         return false;
      }

//*** prevent extrapolation on either end

      if(tsec < tstr || tsec > tstp){
         ierr=2;
         return false;
      }

//*** routine for finding index into table
//*** produces normalized time [0,1,...,8] --> should be 3.5 < tnorm < 4.5

      i  = ((int)Math.round((tsec-tstr)/dlt)+1)-4;
      t0 = (i-1)*dlt+tstr;

//*** boundary conditions

      if(i < 1){
         i  = 1;
         t0 = tstr;
      }else if(i > imx){
         i  = imx;
         t0 = t0mx;
      }
      t = (tsec-t0)/dlt;
      ioff = i-1;

//*** 9-th order interpolator             -- equally-spaced points
//*** time normalized 0,1,...,8           -- should be 3.5 < t < 4.5
//*** ioff is offset into array xs[][]

//*** use stored values if t is exact integer time (avoid division by 0)

      intt = (int)Math.round(t);
      tint = (double)intt;
      if(Math.abs(t-tint) <= EPS) {
         if(intt >= 0 && intt <= 8) {
            xSP3 = xs[intt+1+ioff][iprn];
            ySP3 = ys[intt+1+ioff][iprn];
            zSP3 = zs[intt+1+ioff][iprn];
            tSP3 = ts[intt+1+ioff][iprn];
            if(tSP3 >= BADT){
               ierr=3;
               return false;
            }
         }else{
            System.err.println("index error inside oterp()");
            System.exit(1);
         }

//*** normal interpolation
//*** coefficents, c(), initialized in constructor--see Ben Remondi diss.

      }else{
         powr =1.0;
         xSP3 =0.0;
         ySP3 =0.0;
         zSP3 =0.0;
         tSP3 =0.0;
         for(int j=1; j<=9; j++){
           dt   = t - (double)(j-1);
           powr = dt*powr;
           fac  = 1.0/(c[j]*dt);
           xSP3 = xSP3 + xs[j+ioff][iprn]*fac;
           ySP3 = ySP3 + ys[j+ioff][iprn]*fac;
           zSP3 = zSP3 + zs[j+ioff][iprn]*fac;
           tSP3 = tSP3 + ts[j+ioff][iprn]*fac;
            if(ts[j+ioff][iprn] >= BADT){
               ierr=3;
               return false;
            }
         }
         xSP3 = xSP3*powr;
         ySP3 = ySP3*powr;
         zSP3 = zSP3*powr;
         tSP3 = tSP3*powr;
      }
      ierr=0;
      return true;
   }

//***
//*** constructor
//***

   public SP3set(BufferedReader in, Gtime tStufx) {
      tStuf = tStufx;
      nsat=doSP3Header(in);                              //*** header
      if(nsat > MXPRN){
         System.err.println("Fatal-- nsats=" + nsat);
         System.exit(1);
      }
      for(int i=0; i<=MXTRW; i++){                       //*** zero everything
         for(int j=0; j<=nsat; j++){
            xs[i][j] = 0.0; 
            ys[i][j] = 0.0; 
            zs[i][j] = 0.0; 
            ts[i][j] = 0.0; 
         }
      }
      doSP3Data(in);

//*** finish setting up some index/bookkeeping info

      if(Math.abs( ((tstp-tstr)/(nep-1)) - dlt) > TINY){
         System.err.println("delta t mismatch =" + dlt);
         System.exit(1);
      }
      imx=nep-9+1;
      t0mx=(imx-1)*dlt+tstr;

//*** interpolation coefficients and parameters

      c[1] = 40320.0;
      c[2] = -5040.0;
      c[3] =  1440.0;
      c[4] =  -720.0;
      c[5] =   576.0;
      c[6] =  -720.0;
      c[7] =  1440.0;
      c[8] = -5040.0;
      c[9] = 40320.0;
   }

//***
//*** load methods only accessed thru constructor
//***

   private void doSP3Data(BufferedReader in){

//*** loop over SP3 file data

      String myString;
      String subField;
      int    iprn;
      double x,y,z,t;

      try {
         while( (myString=in.readLine()) != null ){

//*** header

            subField=myString.substring(0,3);
            if(subField.equals("EOF")) {
               break;
            }else{
               nep=nep+1;
               if(nep > MXTRW){
                  System.err.println("Fatal-- # epoch exeeded=" + nep);
                  System.exit(1);
               }
               tstp = eparse(myString);       //*** last epoch (keep updating)
               if(nep == 1)  tstr = tstp;     //*** first epoch
            }

//*** sats                                         *** (P records only)
//***   convert to meters and seconds

            for(int i=1; i<=nsat; i++){
               if( (myString=in.readLine()) != null ){
                  subField=myString.substring(0,1);
                  if(!subField.equals("P")) {
                     System.err.println("Fatal-- missed P rec.");
                     System.exit(1);
                  }

                  subField = myString.substring( 1,4);
                  iprn     = Integer.parseInt(subField.trim());

                  subField = myString.substring( 4,18);
                  x        = Double.valueOf(subField.trim()).doubleValue();
                  xs[nep][iprn] = x*1000.0;

                  subField = myString.substring(18,32);
                  y        = Double.valueOf(subField.trim()).doubleValue();
                  ys[nep][iprn] = y*1000.0;

                  subField = myString.substring(32,46);
                  z        = Double.valueOf(subField.trim()).doubleValue();
                  zs[nep][iprn] = z*1000.0;

                  subField = myString.substring(46,60);
                  t        = Double.valueOf(subField.trim()).doubleValue();
                  ts[nep][iprn] = t/1.0e6;

               }else{
                  System.err.println("Fatal SP3 read\n");
                  in  = null;
                  System.exit(1);
               }
            }
         }//*** endwhile
      }//*** endtry
      catch (IOException e) {
         System.err.println("Error in SP3 r/w\n" + e.toString());
         System.exit(1);
      }
   }

   private double eparse(String myStr){

//*** parse the epoch record in an SP3 format

      int    iyr, imo, idy, ihr, imn;
      double sec;
      String subField;

      subField=myStr.substring(0,1);
      if(!subField.equals("*")) {
         System.err.println("Fatal-- missed epoch rec.");
         System.exit(1);
      }

      subField = myStr.substring( 3, 7);
      iyr       = Integer.parseInt(subField.trim());

      subField = myStr.substring( 7,10);
      imo       = Integer.parseInt(subField.trim());

      subField = myStr.substring(10,13);
      idy       = Integer.parseInt(subField.trim());

      subField = myStr.substring(13,16);
      ihr       = Integer.parseInt(subField.trim());

      subField = myStr.substring(16,19);
      imn       = Integer.parseInt(subField.trim());

      subField = myStr.substring(19,myStr.length());
      sec       = Double.valueOf(subField.trim()).doubleValue();

      return tStuf.civjts(iyr, imo, idy, ihr, imn, sec);
   }

   private int doSP3Header(BufferedReader in){

//*** loop over SP3 file header

      String myString;
      String subField;
      int    icount;
      int    nsat=0;

//*** loop until header end detected

      icount=0;
      try {
         if( (myString=in.readLine()) != null ){icount=icount+1;}

         if( (myString=in.readLine()) != null ){
            icount=icount+1;
            subField = myString.substring(23,38);
            dlt      = Double.p