rescor.cpp

GPS数据预处理软件

   if(roin.epochFlag != 0 && roin.epochFlag != 1) {
      if(Debug) logof << "Found in-line header (dump comments only)" << endl;
      //roin.auxHeader.dump(logof);
      for(int i=0; i<roin.auxHeader.commentList.size(); i++) {
         string s=roin.auxHeader.commentList[i];
         if(Debug) logof << s << endl;
         if(RefPosInput) {
            string t=stripFirstWord(s);
            if(t == string("XYZT")) {
               double x=asDouble(stripFirstWord(s));
               double y=asDouble(stripFirstWord(s));
               double z=asDouble(stripFirstWord(s));
               CurrRef.RxPos.setECEF(x,y,z);
               CurrRef.clk = asDouble(stripFirstWord(s));
            }
            else if(t==string("DIAG")) {
               CurrRef.NPRN = asInt(stripFirstWord(s));
               CurrRef.PDOP = asDouble(stripFirstWord(s));
               CurrRef.GDOP = asDouble(stripFirstWord(s));
               CurrRef.RMS = asDouble(stripFirstWord(s));
               CurrRef.valid = true;
//logof << "Found position:\n" << CurrRef.RxPos.printf("%.4x %.4y %.4z\n");
            }
         }
      }
      return 0;
   }
   
   // --------------------------------------------------------------------
   // Save the time tag (wait to define until after in-line header info)
   CurrentTime = roin.time;

   // --------------------------------------------------------------------
   // save the raw data, if they're not in the output
   DataStoreMap.clear();
   if((inL1>-1 && otL1==-1) || (inL2>-1 && otL2==-1) ||
      (inP1>-1 && (otP1==-1 || (Cforce && otC1==-1))) || (inP2>-1 && otP2==-1)) {
         SaveData(roin, rhead, inL1, inL2, inP1, inP2);
   }
   
   return 0;
}
catch(Exception& e) { GPSTK_RETHROW(e); }
catch(exception& e) { Exception E("std except: "+string(e.what())); GPSTK_THROW(E); }
catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
}

//------------------------------------------------------------------------------------
// before writing out header (pass output header)
int RCRinexEditor::BeforeWritingHeader(RinexObsHeader& rhout) throw(Exception)
{
try {
   return 0;
}
catch(Exception& e) { GPSTK_RETHROW(e); }
catch(exception& e) { Exception E("std except: "+string(e.what())); GPSTK_THROW(E); }
catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
}

//------------------------------------------------------------------------------------
// before writing out filled header (pass output header)
int RCRinexEditor::BeforeWritingFilledHeader(RinexObsHeader& rhout) throw(Exception)
{
try {
   if(headRAIM) {
         // put average RAIM position in header
      rhout.antennaPosition[0] = ARSX.Average();
      rhout.antennaPosition[1] = ARSY.Average();
      rhout.antennaPosition[2] = ARSZ.Average();
      rhout.valid |= RinexObsHeader::antennaPositionValid;
      if(Verbose) logof << "Average RAIM solution (" << ARSX.N()
         << ") at time " << CurrentTime << " : "
         << " " << fixed << setw(16) << setprecision(6) << ARSX.Average()
         << " +/- " << scientific << setw(8) << setprecision(2) << ARSX.StdDev()
         << ", " << fixed << setw(16) << setprecision(6) << ARSY.Average()
         << " +/- " << scientific << setw(8) << setprecision(2) << ARSY.StdDev()
         << ", " << fixed << setw(16) << setprecision(6) << ARSZ.Average()
         << " +/- " << scientific << setw(8) << setprecision(2) << ARSZ.StdDev()
         << endl;
   }

   if(Verbose) logof << "\nHere is the output header after optional records filled\n";
   rhout.dump(logof);

   return 0;
}
catch(Exception& e) { GPSTK_RETHROW(e); }
catch(exception& e) { Exception E("std except: "+string(e.what())); GPSTK_THROW(E); }
catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
}

//------------------------------------------------------------------------------------
// just before writing output obs (pass output obs)
// return value of BeforeWritingObs determines what is written:
// if return <0 abort
//            0 write nothing
//            1 write the obs data structure (note that if epochFlag==4,
//               this will result in in-line header information only)
//            2 write both header data (in auxHeader) and obs data
int RCRinexEditor::BeforeWritingObs(RinexObsData& roout) throw(Exception)
{
try {
   int i;
      // what to do with other epochFlags (in-line header information, etc)
   if(roout.epochFlag != 0 && roout.epochFlag != 1) return 0;

      // save the data, if they're in the output
   if(otL1>-1 || otL2>-1 || otP1>-1 || otP2>-1)
      SaveData(roout, rheadout, otL1, otL2, otP1, otP2);

      // update the receiver position (via RAIM or file input)
   if(UpdateRxPosition()) {
      logof << "Failed to update Rx position at time " << CurrentTime << endl;
      cerr << "Failed to update Rx position at time " << CurrentTime << endl;
      return -1;
   }

      // compute new OTs, and add to obs
   ComputeNewOTs(roout);

      // write RAIM position solution to in-line header
   if(outRef && (HaveRAIM || !RefPosFile.empty())) {
      ostringstream stst1,stst2;
      roout.auxHeader.clear();
      stst1 << "XYZT";
      stst1 << fixed << " " << setw(13) << setprecision(3) << CurrRef.RxPos.X();
      stst1 << fixed << " " << setw(13) << setprecision(3) << CurrRef.RxPos.Y();
      stst1 << fixed << " " << setw(13) << setprecision(3) << CurrRef.RxPos.Z();
      stst1 << fixed << " " << setw(13) << setprecision(3) << CurrRef.clk;
      roout.auxHeader.commentList.push_back(stst1.str());
      if(Verbose)
         logof << "RAIM output: " << roout.time.printf("%02M:%04.1f ") << stst1.str();

      //for(Nsvs=0,i=0; i<Sats.size(); i++) if(Sats[i].sat > 0) Nsvs++;
      //PDOP = RSS(prsol.Covariance(0,0),
      //      prsol.Covariance(1,1),prsol.Covariance(2,2));
      //GDOP = RSS(PDOP, prsol.Covariance(3,3));
      //rms = prsol.RMSResidual;
      stst2 << "DIAG";
      stst2 << " " << setw(2) << CurrRef.NPRN
         << " " << fixed << setw(5) << setprecision(2) << CurrRef.PDOP
         << " " << fixed << setw(5) << setprecision(2) << CurrRef.GDOP
         << " " << fixed << setw(9) << setprecision(3) << CurrRef.RMS
         << " (N,P-,G-Dop,RMS)";
      roout.auxHeader.commentList.push_back(stst2.str());
      if(Verbose) logof << " " << stst2.str() << endl;
      roout.auxHeader.valid |= RinexObsHeader::commentValid;

      return 4;         // write both header (with epochFlag=4) and obs data
   }

   return 0;
}
catch(Exception& e) { GPSTK_RETHROW(e); }
catch(exception& e) { Exception E("std except: "+string(e.what())); GPSTK_THROW(E); }
catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
}

//------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------
void SaveData(const RinexObsData& rod, const RinexObsHeader& rhd,
   int xL1, int xL2, int xP1, int xP2) throw(Exception)
{
try {
   RinexSatID sat;
   RinexObsData::RinexObsTypeMap otmap;
   RinexObsData::RinexSatMap::const_iterator it;
   RinexObsData::RinexObsTypeMap::const_iterator jt;
   map<RinexSatID,RCData>::const_iterator kt;

   for(it=rod.obs.begin(); it != rod.obs.end(); ++it) { // loop over satellites
      sat = RinexSatID(it->first.id,SatID::systemGPS);
      otmap = it->second;
      // find the saved input data for this sat, if any
      kt = DataStoreMap.find(sat);
      if(kt != DataStoreMap.end()) DataStore=kt->second;

      if(xL1>-1 && (jt=otmap.find(rhd.obsTypeList[xL1])) != otmap.end()) {
         DataStore.L1 = jt->second.data;
         DataStore.LL1 = int(jt->second.lli);
      }
      if(xL2>-1 && (jt=otmap.find(rhd.obsTypeList[xL2])) != otmap.end()) {
         DataStore.L2 = jt->second.data;
         DataStore.LL2 = int(jt->second.lli);
      }
      if(xP1>-1 && (jt=otmap.find(rhd.obsTypeList[xP1])) != otmap.end()) {
         DataStore.P1 = jt->second.data;
      }
      if(xP2>-1 && (jt=otmap.find(rhd.obsTypeList[xP2])) != otmap.end()) {
         DataStore.P2 = jt->second.data;
      }
      DataStoreMap[sat] = DataStore;
   }  // end loop over sats
}
catch(Exception& e) { GPSTK_RETHROW(e); }
catch(exception& e) { Exception E("std except: "+string(e.what())); GPSTK_THROW(E); }
catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
}

//------------------------------------------------------------------------------------
// fill global data CurrRef
int UpdateRxPosition(void) throw(Exception)
{
try {
   int iret,i;
   double rho;
   Xvt xvt;
   RinexSatID sat;
   CorrectedEphemerisRange CER;

      // compute a RAIM solution, add it to average
   HaveRAIM = false;
   map<RinexSatID,RCData>::const_iterator kt;
   if(doRAIM) {
      Sats.clear();
      PRange.clear();

         //map<RinexSatID,RCData> DataStoreMap;
      for(kt=DataStoreMap.begin(); kt != DataStoreMap.end(); kt++) {
         if(kt->second.P1 == 0 || kt->second.P2 == 0) continue;
         sat = kt->first;
         if(minElev > 0.0 && CurrRef.valid) {
            try {
               if(SP3EphList.size() > 0)
                  rho = CER.ComputeAtReceiveTime(CurrentTime, xvt, sat,
                        SP3EphList);
               else if(BCEphList.size() > 0)
                  rho = CER.ComputeAtReceiveTime(CurrentTime, xvt, sat,
                        BCEphList);
               else continue;
            }
            catch(gpstk::EphemerisStore::NoEphemerisFound& e) {
               continue;
            }
         }
         Sats.push_back(sat);
         PRange.push_back(if1r*kt->second.P1+if2r*kt->second.P2);
      }

      if(SP3EphList.size() > 0)
         iret = prsol.RAIMCompute(CurrentTime, Sats, PRange, SP3EphList, &ggtm);
      else if(BCEphList.size() > 0)
         iret = prsol.RAIMCompute(CurrentTime, Sats, PRange, BCEphList, &ggtm);
      else iret = -4;
         //  2  failed to find a good solution (RMS residual or slope exceed limits)
         //  1  solution is suspect (slope is large)
         //  0  ok
         // -1  failed to converge
         // -2  singular problem
         // -3  not enough good data to form a RAIM solution
         //     (the 4 satellite solution might be returned - check isValid())
         // -4  ephemeris not found for one or more satellites
      HaveRAIM = (iret==0 || iret==1);
      if(HaveRAIM) {
         if(Verbose) {                          // output results and return value
            int Nsvs;
            for(Nsvs=0,i=0; i<Sats.size(); i++) if(Sats[i].id > 0) Nsvs++;
            logof << "RPF " << setw(2) << Sats.size()-Nsvs
               << " " << setw(4) << CurrentTime.GPSfullweek() << fixed
               << " " << setw(10) << setprecision(3) << CurrentTime.GPSsecond()
               << " " << setw(2) << Nsvs
               << " " << setw(16) << setprecision(6) << prsol.Solution(0)
               << " " << setw(16) << setprecision(6) << prsol.Solution(1)
               << " " << setw(16) << setprecision(6) << prsol.Solution(2)
               << " " << setw(16) << setprecision(6) << prsol.Solution(3)
               << " " << setw(16) << setprecision(6) << prsol.RMSResidual
               << " " << fixed << setw(7) << setprecision(1) << prsol.MaxSlope
               << " " << prsol.NIterations
               << " " << scientific
               << setw(8) << setprecision(2) << prsol.Convergence;
            for(i=0; i<Sats.size(); i++) logof << " " << setw(3) << Sats[i].id;
            logof << " (" << iret << ")" << (prsol.isValid() ? " V":" NV") << endl;
         }

         CurrRef.RxPos.setECEF(prsol.Solution(0),prsol.Solution(1),
            prsol.Solution(2));
         CurrRef.valid = true;
         CurrRef.clk = prsol.Solution(3);
         CurrRef.NPRN = prsol.Nsvs;
         CurrRef.PDOP = RSS(prsol.Covariance(0,0),prsol.Covariance(1,1),
            prsol.Covariance(2,2));
         CurrRef.GDOP = RSS(CurrRef.PDOP, prsol.Covariance(3,3));
         CurrRef.RMS = prsol.RMSResidual;
         if(headRAIM) {       // add to average
            ARSX.Add(CurrRef.RxPos.X());
            ARSY.Add(CurrRef.RxPos.Y());
            ARSZ.Add(CurrRef.RxPos.Z());
         }
         inPS = 1;
      }
      else {                     // RAIM failed
         if(Verbose) {
            logof << "RAIM failed at " << CurrentTime << " : returned '";
            if(iret == 2) logof << "failed to find a good solution "
               << "(RMS residual or slope exceed limits)";
            if(iret == -1) logof << "failed to converge";
            if(iret == -2) logof << "singular problem";
            if(iret == -3) logof << "not enough good data to form a RAIM solution";
            if(iret == -4) {
               logof << "ephemeris not found for satellite";
               for(i=0; i<Sats.size(); i++) {
                  if(Sats[i].id < 0) {
                     Sats[i].id *= -1;
                     logof << " " << Sats[i];
                  }
               }
            }
            logof << "'." << endl;
         }
         inPS=-1;
      }
   }
   else if(!RefPosFile.empty()) { // update RxPos from map
      map<DayTime,RefPosData>::iterator ite;
      ite = RefPosMap.lower_bound(CurrentTime);
      // ite points to first element with value >= CurrentTime
      if(ite == RefPosMap.end() || fabs(ite->first - CurrentTime) > 0.1*RefPosMapDT) {
         if(Verbose) logof << "No Rx position found at " << CurrentTime << endl;
         CurrRef.valid = false;
         inPS = -1;
      }
      else {
         CurrRef.RxPos = ite->second.RxPos;
         CurrRef.clk = ite->second.clk;
         CurrRef.NPRN = ite->second.NPRN;
         CurrRef.PDOP = ite->second.PDOP;
         CurrRef.GDOP = ite->second.GDOP;
         CurrRef.RMS = ite->second.RMS;
         CurrRef.valid = true;
         inPS = 1;
      }
   }

   if(Debug && inPS > -1) {
      logof << "RxPos " << Curre