jave-ppp.txt

精密单点定位程序

                  allBC[myprn][nrwBC[myprn]]=myBC;

//*** supress close updates (1/2 hour)

               }else{                                //*** these got preempted
                  //*** null action                  //*** e.g. --> 16 sec.
               }
            }//*** endelseif (not first entry)

         }//*** endwhile
      }//*** endtry
      catch (IOException e) {
         System.err.println("Error in data read\n" + e.toString());
         System.exit(1);
      }
   }

   private BCstuf readEntry(BufferedReader in, String myString, int myid) 
                                 throws IOException {

//*** process first string and 7 remaining lines

      BCstuf myBC = new BCstuf(myid);
      myBC.put0BC(myString, myid, tStuf);

      myString=in.readLine();
      myBC.put1BC(myString);

      myString=in.readLine();
      myBC.put2BC(myString);

      myString=in.readLine();
      myBC.put3BC(myString);

      myString=in.readLine();
      myBC.put4BC(myString);

      myString=in.readLine();
      myBC.put5BC(myString);

      myString=in.readLine();
      myBC.put6BC(myString);

      myString=in.readLine();
      myBC.put7BC(myString);

      return myBC;
   }

   private void doNavHeader(BufferedReader in){

//*** loop over nav file header

      String myString;
      String typeField;
      String subField;

//*** loop until header end detected

      try {
         while( (myString=in.readLine()) != null ){
            typeField=myString.substring(60,myString.length());
            typeField=typeField.trim();
            if(typeField.equals("ION ALPHA")) {
               subField = myString.substring( 2, 14).replace('D','e');
               alfa1    = Double.valueOf(subField.trim()).doubleValue();

               subField = myString.substring(14, 26).replace('D','e');
               alfa2    = Double.valueOf(subField.trim()).doubleValue();

               subField = myString.substring(26, 38).replace('D','e');
               alfa3    = Double.valueOf(subField.trim()).doubleValue();

               subField = myString.substring(38, 50).replace('D','e');
               alfa4    = Double.valueOf(subField.trim()).doubleValue();

               BCSiono = true;
            }
            if(typeField.equals("ION BETA")) {
               subField = myString.substring( 2, 14).replace('D','e');
               beta1    = Double.valueOf(subField.trim()).doubleValue();

               subField = myString.substring(14, 26).replace('D','e');
               beta2    = Double.valueOf(subField.trim()).doubleValue();

               subField = myString.substring(26, 38).replace('D','e');
               beta3    = Double.valueOf(subField.trim()).doubleValue();

               subField = myString.substring(38, 50).replace('D','e');
               beta4    = Double.valueOf(subField.trim()).doubleValue();

               BCSiono = true;
            }
            if(typeField.equals("END OF HEADER")) {return;}
         }
      } 
      catch (IOException e) {
         System.err.println("Error in copy\n" + e.toString());
         System.exit(1);
      } 
   }

//***
//*** ionosphere methods
//***

   public boolean hasBCSiono() {return BCSiono;}

   public double l1BCion(double tr, RVec myV, Pos rxPos){

//*** broadcast iono (c.v. icd-200)

//*** input time:    seconds of ut (or gps time of day)      (machts nicht)
//*** output:        l1 time delay in seconds

      double az, vas, glas, glos;
      double psi, glai, gloi, glam, tlocal, f, amp, per, x, delay;

//*** divide by pi to convert radians to semicircles

      myV.xyzlhs(rxPos);
      az   = myV.getAzRV();               //*** radians here
      vas  = myV.getVaRV()/gpspi;
      glas = rxPos.getGlaGP()/gpspi;
      glos = rxPos.getGloGP()/gpspi;

      psi  = 0.0137/(vas+0.11)-0.022;
      glai = glas+psi*Math.cos(az);
      if(glai < -0.416) glai = -0.416;
      if(glai >  0.416) glai =  0.416;
      gloi = glos+psi*Math.sin(az)/Math.cos(glai*gpspi);
      glam = glai+0.064*Math.cos((gloi-1.617)*gpspi);

//*** local time from gps or ut time

      tlocal=43200.0*gloi+tr;
      if(tlocal <      0.0) tlocal=tlocal+86400.0;
      if(tlocal >= 86400.0) tlocal=tlocal-86400.0;

//*** magnification factor, amplitude, and period

      f=1.0+16.0*(0.53-vas)*(0.53-vas)*(0.53-vas);     //*** obliquity

      amp=alfa1               +
          alfa2*glam          +
          alfa3*glam*glam     +
          alfa4*glam*glam*glam;
      if(amp < 0.0) {
         System.out.println("amp limit="+amp);
         amp=0.0;
      }

      per=beta1               +
          beta2*glam          +
          beta3*glam*glam     +
          beta4*glam*glam*glam;
      if(per < 72000.0) per=72000.0;
 
      x=2.0*gpspi*(tlocal-50400.0)/per;

      delay=f*5.0e-9;

      if(Math.abs(x) < 1.57)
                     delay = delay + f*amp*( 1.0 - (x*x/2.0) + (x*x*x*x/24.0));
      return delay;
   }

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

   public boolean BCcutoff (int prn, double tr, Pos rxPos, Modes myModes, 
                            SP3set mySP3) {
      
//*** find vert angle and test on cutoff

//*** (debug)streamline -- forget light loop

      boolean losat;
      RVec   myV;

      myV = ltloop(prn, tr, rxPos, myModes, mySP3);
      myV.xyzlhs(rxPos);

//*** cutoff in degrees

      if (myV.getVaRV()*RAD < myModes.getvCut()) {
         losat=true;
      }else{
         losat=false;
      }

      return losat;
   }

   public PosD p1xyz(int nobs, double p1s[], double p2s[], int prns[], 
                     double tr, Pos rxPos, Modes myModes, SP3set mySP3) {
      
//*** solve for xyz given p1 observations
//***     tr is time at receiver
//***     code not iterative -- bad sats kept  (debug?enhance?)
//***     mySP3 may be null if not precise orbit mode

      int    prn;
      double p1,p2,p0,dx,dy,dz,wt,cmo;
      double pdop,hdop,vdop;
      RVec   myV;

      int nunk=4;

//*** initialize arrays and normals           (fortran indexing)

      double an[][] = new double [nunk+1][nunk+1];       //*** normals
      double u[]    = new double [nunk+1];               //*** right hand side
      double x[]    = new double [nunk+1];               //*** unknowns
      double c[]    = new double [nunk+1];               //*** a-matrix coeff.
      int    ic[]   = new int    [nunk+1];               //*** coeff. indicies
      double ro[][] = new double [   3+1][nunk+1];       //*** rot.mat for dop's
      double dp[][] = new double [   3+1][   3+1];       //*** l.h.s. dop's

      if(!Matrix.nitial(an, u)){
         System.err.println("Failed to initialize normals");
         System.exit(1);
      }

      for(int i=1; i<ic.length; i++){ ic[i]=i; }    //*** monotonic increase

//*** accumulate normals

      wt   = 1.0;
      c[4] = 1.0;                            //*** clock in units of meters

      for(int i=1; i<=nobs; i++){
         prn  = prns[i];
         if(myModes.isdFreq()){
            myV = ltloop(prn, tr, rxPos, myModes, mySP3);
            p1  = p1s[i]+sol*bcclok;           //*** correct p/r sv clock error
            p2  = p2s[i]+sol*bcclok;           //*** correct p/r sv clock error
            p1  = (p2-GAMMA*p1)/GAMMA1;        //*** iono correction
 
         }else if(myModes.isbIon()){
            myV = ltloop(prn, tr, rxPos, myModes, mySP3);
            p1  = p1s[i]+sol*bcclok;           //*** correct p/r sv clock error
            p1  = p1    -sol*l1BCion(tr,myV,rxPos);       //*** iono correction

         }else{
            myV  = ltloop(prn, tr, rxPos, myModes, mySP3);
            p1   = p1s[i]+sol*bcclok;         //*** correct p/r sv clock error
         }
         if(myModes.isTrop()){
            myV.xyzlhs(rxPos);
            p1=p1-myMet.tropdelay(rxPos.getGlaGP(), rxPos.getEhtGP(),
                                  myV.getVaRV(), tr, tStuf);
         }
         dx   = myV.getPxRV();
         dy   = myV.getPyRV();
         dz   = myV.getPzRV();
         p0   = myV.getRhoRV();
         cmo  = p0-p1;
         c[1] = -dx/p0;
         c[2] = -dy/p0;
         c[3] = -dz/p0;

         if(!Matrix.normal(an, u, c, ic, cmo, wt, nunk)){
            System.err.println("Failed to accumulate normals");
            System.exit(1);
         }
      }

//*** solve normals

      if(!Matrix.nfill(an)){
         System.err.println("Failed to fill lower triangle of normals");
         System.exit(1);
      }

      if(!Matrix.invert(an)){
         System.err.println("Failed to invert normals");
         System.exit(1);
      }

      if(!Matrix.av(an, u, x)){
         System.err.println("Failed to solve normals");
         System.exit(1);
      }

//*** dop's

      pdop = Math.sqrt(an[1][1]+an[2][2]+an[3][3]);              //*** pdop
      if(pdop > 99.9) pdop=99.9;                                 //*** limit

      ro = rxPos.rotmat();                                       //*** geo->lhs

      hdop = Math.sqrt(Matrix.rcrt(Matrix.rowcopy(ro,1), an)
                      +Matrix.rcrt(Matrix.rowcopy(ro,2), an));
      if(hdop > 99.9) hdop=99.9;                                 //*** limit

      vdop = Math.sqrt(Matrix.rcrt(Matrix.rowcopy(ro,3), an));
      if(vdop > 99.9) vdop=99.9;                                 //*** limit

//*** apply to a priori estimate

      return new PosD(rxPos.getxPos()-x[1],
                      rxPos.getyPos()-x[2],
                      rxPos.getzPos()-x[3],
                      pdop, hdop, vdop);
   }

   public RVec ltloop(int prn, double tr, Pos rxPos, Modes myModes, 
                      SP3set mySP3){
      
//*** solve light loop for a given prn and receiver
//***     tr is time at receiver
//***     mySP3 may be null if not precise orbit

      double ttbar,tt,dt,sag,rho,dtr;
      double px,py,pz;
      PosT   svPos=null;

//*** iterate the light loop a fixed number of times  (2 for +/- 0.01 mm)

      dt=0.075;                                  //*** approx transit time
      for(int i=0; i<2; i++){

//*** compute broadcast position  (incl. clock corrn.)

         svPos=BCpos(prn, tr-dt, myModes);

//*** range vector

         px = svPos.getxPosT() - rxPos.getxPos();
         py = svPos.getyPosT() - rxPos.getyPos();
         pz = svPos.getzPosT() - rxPos.getzPos();

         rho=Math.sqrt(px*px + py*py + pz*pz);
         dt=rho/sol;                             //*** updated transit time
      }
      sag=we*dt;                                 //*** apply sagnac effect
      bcclok = svPos.getdtPosT();                //*** bcclok is a global

//*** precise orbit -- override broadcast position and clock

      if( myModes.ispOrb() ) {
         if(mySP3.oterp(prn, tr-dt)){
            svPos.putxPosT ( mySP3.gSP3x() );         //*** update sat pos
            svPos.putyPosT ( mySP3.gSP3y() );
            svPos.putzPosT ( mySP3.gSP3z() );
            dtr = svPos.getdtrPosT();                 //*** relativistic
            svPos.putdtPosT( mySP3.gSP3t() + dtr);    //*** sat clock + relativ
            bcclok = svPos.getdtPosT();               //*** bcclok is a global
         }
      }

      return new RVec(svPos.getxPosT() + sag*svPos.getyPosT() - rxPos.getxPos(),
                      svPos.getyPosT() - sag*svPos.getxPosT() - rxPos.getyPos(),
                      svPos.getzPosT()                        - rxPos.getzPos());
   } 

   public boolean BChealthy (int prn, double tsec) {
      
//*** find health of broadcast orbit (BC) for a prn and time

      int mydx;
      boolean healthy;

      mydx=locBC(prn, tsec);               //** look up row in BC table
      if(mydx <= -1){
         System.err.println("Index prob in BCpos() " + prn + "  " + tsec);
         System.exit(1);
      }

//*** lookup health

      if (allBC[prn][mydx].gBCsvhl() > 0.0) {
         healthy=false;
      }else{
         healthy=true;
      }

      return healthy;
   }

   public PosT BCpos (int prn, double tsec, Modes myModes) {
      
//*** compute pos of broadcast orbit (BC) for a prn and time

      int mydx=locBC(prn, tsec);               //** look up row in BC table
      if(mydx <= -1){
         System.err.println("Index prob in BCpos() " + prn + "  " + tsec);
         System.exit(1);
      }

//*** convert jts time to sow and compute broadcast position

      return allBC[prn][mydx].bcxyzt(tStuf.jtssow(tsec), myModes);
   }

   public int locBC(int prn, double tsec) {

//*** return index to BC array, lookup using jts time (seconds)
//*** returns index of nearest jts version of t0c

      int mxrow;
      
      if(tsec <= allBC[prn][ 1   ].gBCjts()){ return 1; }      //*** first row
      mxrow = nrwBC[prn];