estimation.cpp

linux的gps应用

   //LabelledVector LCov(StateNL,Sig);
   //LCov.setw(20).setprecision(6);
   ////oflog << "Covariance\n" << LCov << endl;
   //oflog << "Sigma\n" << LCov << endl;

      // output baselines
   Position BL;
   for(i=0; i<CI.OutputBaselines.size(); i++) {
         // dependent on the order given in ComputeSingleDifferences()
      string one=StringUtils::word(CI.OutputBaselines[i],0,'-');
      string two=StringUtils::word(CI.OutputBaselines[i],1,'-');
      BL = Stations[one].pos - Stations[two].pos;
      oflog << "Baseline " << CI.OutputBaselines[i]
         << " " << BL.printf("%16.6x %16.6y %16.6z")
         << " " << f166 << BL.getRadius() << endl;
      if(CI.Screen) cout << "Baseline " << CI.OutputBaselines[i]
         << " " << BL.printf("%16.6x %16.6y %16.6z")
         << " " << f166 << BL.getRadius() << endl;
         // offset - if one was input
      if(CI.OutputBaselineOffsets[i].mag() >= 0.01) {
         oflog << " Offset  " << CI.OutputBaselines[i]
            << " " << f166 << BL.X() - CI.OutputBaselineOffsets[i][0]
            << " " << f166 << BL.Y() - CI.OutputBaselineOffsets[i][1]
            << " " << f166 << BL.Z() - CI.OutputBaselineOffsets[i][2]
            << " " << f166 << BL.getRadius() - CI.OutputBaselineOffsets[i].mag()
            << endl;
         if(CI.Screen) cout << " Offset  " << CI.OutputBaselines[i]
            << " " << f166 << BL.X() - CI.OutputBaselineOffsets[i][0]
            << " " << f166 << BL.Y() - CI.OutputBaselineOffsets[i][1]
            << " " << f166 << BL.Z() - CI.OutputBaselineOffsets[i][2]
            << " " << f166 << BL.getRadius() - CI.OutputBaselineOffsets[i].mag()
            << endl;
      }
   }

      // compute residuals of fit and output
   double rmsrof = RMSResidualOfFit(N,dX,final);
   oflog << "RES " << (final ? "final " : "" ) << "total RMS = "
      << f82s << rmsrof << endl;

}
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); }
}

//------------------------------------------------------------------------------------
// called by Estimation()
// return -1  quit now
//         0   go on
//         1   reached convergence and don't fix biases
//         2   reached last iteration and don't fix biases
//         4   1 and/or 2 and fix biases, i.e. fix the biases then quit
int IterationControl(int iter_n)
{
try {
   int done=0;
   double converge;

   // has it converged?
   converge = norm(dX);
   if(converge <= CI.convergence) {
      oflog << "DDBase finds convergence: " << iter_n << " iterations"
            << ", convergence criterion = " << scientific << setprecision(3)
            << converge << " m; (" << CI.convergence << " m)" << endl;
      if(CI.Screen)
         cout << "DDBase finds convergence: " << iter_n << " iterations"
            << ", convergence criterion = " << scientific << setprecision(3)
            << converge << " m; (" << CI.convergence << " m)" << endl;
      done += 1;
   }

   // have we reached the last iteration?
   if(iter_n == CI.nIter) {
      oflog << "DDBase finds last iteration: " << iter_n << " iterations"
            << ", convergence criterion = " << scientific << setprecision(3)
            << converge << " m; (" << CI.convergence << " m)" << endl;
      if(CI.Screen)
         cout << "DDBase finds last iteration: " << iter_n << " iterations"
            << ", convergence criterion = " << scientific << setprecision(3)
            << converge << " m; (" << CI.convergence << " m)" << endl;
      done += 2;
   }
   
   if(!done && CI.Verbose) {
      oflog << "DDBase: " << iter_n << " iterations"
            << ", convergence criterion = " << scientific << setprecision(3)
            << converge << " m; (" << CI.convergence << " m)" << endl;
      if(CI.Screen)
         cout << "DDBase: " << iter_n << " iterations"
            << ", convergence criterion = " << scientific << setprecision(3)
            << converge << " m; (" << CI.convergence << " m)" << endl;
   }

   // if the previous iteration fixed the biases, we are done no matter what
   if(Biasfix) return 5;

   if(CI.FixBiases && done)  {
      Biasfix = true;
      return 4;                     // signals one more iteration
   }

   return done;
}
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); }
}

//------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------
// Utilities - use consistently throughout! these three routines must change together.
string ComposeName(const string& site1,
                   const string& site2,
                   const GSatID& sat1,
                   const GSatID& sat2)
{
try {
   //RinexSatID rsat1=sat2,rsat2=sat2;   // use RinexSatID to get the fill char == '0'
   return ( site1 + string("-") + site2 + string("_")
      //+ rsat1.toString() + string("-") + rsat2.toString() );
      //+ sat1.toString() + string("-") + sat2.toString() );
      + StringUtils::asString(sat1) + string("-") + StringUtils::asString(sat2) );
}
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); }
}
//------------------------------------------------------------------------------------
string ComposeName(const DDid& ddid)
{
try {
   //ostringstream oss;
   //if(ddid.ssite > 0) oss << ddid.site1 << "-" << ddid.site2 << "_";
   //else               oss << ddid.site2 << "-" << ddid.site1 << "_";
   //if(ddid.ssat  > 0) oss << ddid.sat1  << "-" << ddid.sat2;
   //else               oss << ddid.sat2  << "-" << ddid.sat1;
   //return(oss.str());
   if(ddid.ssite > 0) {
      if(ddid.ssat > 0) 
         return ComposeName(ddid.site1,ddid.site2,ddid.sat1,ddid.sat2);
      else 
         return ComposeName(ddid.site1,ddid.site2,ddid.sat2,ddid.sat1);
   }
   else {
      if(ddid.ssat > 0) 
         return ComposeName(ddid.site2,ddid.site1,ddid.sat1,ddid.sat2);
      else 
         return ComposeName(ddid.site2,ddid.site1,ddid.sat2,ddid.sat1);
   }
}
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 DecomposeName(const string& label,
                   string& site1,
                   string& site2,
                   GSatID& sat1,
                   GSatID& sat2)
{
try {
   string copy=label;
   //oflog << "Decompose found " << label << " = ";
   site1 = StringUtils::stripFirstWord(copy,'-');
   //oflog << site1;
   site2 = StringUtils::stripFirstWord(copy,'_');
   //oflog << " " << site2;
   sat1.fromString(StringUtils::stripFirstWord(copy,'-'));
   //oflog << " " << sat1;
   sat2.fromString(copy);
   //oflog << " " << sat2 << endl;
}
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 OutputFinalResults(int iret)
{
try {
   int i,j,len;
   string site1,site2;
   GSatID sat1,sat2;
   format f133(13,3),f166(16,6);

   oflog << "Final Solution:" << endl;

   if(iret != -2) {

      if(CI.NRZDintervals > 0) {
         oflog << "Residual zenith tropospheric delays (m) with sigma" << endl;
         for(i=0; i<NState; i++) {
            DecomposeName(StateNL.getName(i), site1, site2, sat1, sat2);
            if(site2.substr(0,3) != string("RZD")) continue;
            oflog << site1 << " : trop delay (m) #" << site2.substr(3,site2.size()-3)
               << " " << f133 << State(i)
               << " " << f133 << SQRT(Cov(i,i))
               << endl;
         }
         oflog << endl;
      }

      oflog << "Biases (cycles) with sigma" << endl;
      for(i=0; i<NState; i++) {
         DecomposeName(StateNL.getName(i), site1, site2, sat1, sat2);
         if(site2.size() ==0 || sat1.id == -1 || sat2.id == -1) continue;
         oflog << StateNL.getName(i)
            << " " << f133 << BiasState(i)/wl1
            << " " << f133 << SQRT(BiasCov(i,i))/wl1
            << endl;
      }
      oflog << endl;

      // output position and covariance for input to later adjustment
      oflog << "Final covariance and position solutions:\n";
      for(len=0,j=0; j<NState; j++) {
         DecomposeName(StateNL.getName(j), site1, site2, sat1, sat2);
         if(site2 == string("X") || site2 == string("Y") || site2 == string("Z")) {
            if(len==0) {
               len = StateNL.getName(j).size();
               oflog << setw(len) << ' ';
               if(len < 16) len=16;
            }
            oflog << setw(len) << StateNL.getName(j);
         }
      }
      oflog << setw(len) << "Position" << endl;
      for(i=0; i<NState; i++) {
         DecomposeName(StateNL.getName(i), site1, site2, sat1, sat2);
         if(site2!=string("X") && site2!=string("Y") && site2!=string("Z"))
            continue;
         oflog << StateNL.getName(i);
         for(j=0; j<NState; j++) {
            string site22,site11;
            GSatID sat11,sat22;
            DecomposeName(StateNL.getName(j), site11, site22, sat11, sat22);
            if(site22==string("X") || site22==string("Y") || site22==string("Z"))
               oflog << scientific << setw(len) << setprecision(6) << Cov(i,j);
         }
         if(site2 == string("X")) oflog << fixed << setw(len)
            << setprecision(6) << Stations[site1].pos.X();
         if(site2 == string("Y")) oflog << fixed << setw(len)
            << setprecision(6) << Stations[site1].pos.Y();
         if(site2 == string("Z")) oflog << fixed << setw(len)
            << setprecision(6) << Stations[site1].pos.Z();
         oflog << endl;
      }
      oflog << endl;

      // output position and sigmas for all non-fixed positions
      map<string,Station>::const_iterator it;
      for(it=Stations.begin(); it != Stations.end(); it++) {
         oflog << it->first << ": " << (it->second.fixed ? "    Fixed" : "Estimated")
            << " Position "
            << Stations[it->first].pos.printf("%16.6x %16.6y %16.6z") << endl;
         if(!(Stations[it->first].fixed)) {
            oflog << it->first << ": Estimated   Sigmas";
            i = StateNL.index(it->first + string("-X"));
            oflog << " " << f166 << SQRT(Cov(i,i));
            i = StateNL.index(it->first + string("-Y"));
            oflog << " " << f166 << SQRT(Cov(i,i));
            i = StateNL.index(it->first + string("-Z"));
            oflog << " " << f166 << SQRT(Cov(i,i));
            oflog << endl;
         }
      }

      // do for all baselines
      for(i=0; i<CI.OutputBaselines.size(); i++) {
         string one=StringUtils::word(CI.OutputBaselines[i],0,'-');
         string two=StringUtils::word(CI.OutputBaselines[i],1,'-');
         Position BL = Stations[one].pos - Stations[two].pos;
         oflog << "Final Baseline " << CI.OutputBaselines[i]
            << " " << BL.printf("%16.6x %16.6y %16.6z")
            << " " << f166 << BL.getRadius() << endl;
         if(CI.Screen)
            cout << "Final Baseline " << CI.OutputBaselines[i]
            << " " << BL.printf("%16.6x %16.6y %16.6z")
            << " " << f166 << BL.getRadius() << endl;

            // offset - if one was input
         if(CI.OutputBaselineOffsets[i].mag() >= 0.01) {
            oflog << "Final  Offset  " << CI.OutputBaselines[i]
               << " " << f166 << BL.X() - CI.OutputBaselineOffsets[i][0]
               << " " << f166 << BL.Y() - CI.OutputBaselineOffsets[i][1]
               << " " << f166 << BL.Z() - CI.OutputBaselineOffsets[i][2]
               << " " << f166 << BL.getRadius() - CI.OutputBaselineOffsets[i].mag()
               << endl;
            if(CI.Screen)
               cout << "Final  Offset  " << CI.OutputBaselines[i]
               << " " << f166 << BL.X() - CI.OutputBaselineOffsets[i][0]
               << " " << f166 << BL.Y() - CI.OutputBaselineOffsets[i][1]
               << " " << f166 << BL.Z() - CI.OutputBaselineOffsets[i][2]
               << " " << f166 << BL.getRadius() - CI.OutputBaselineOffsets[i].mag()
               << endl;
         }
      }
   }
   oflog << "Data Totals: " << NEp << " epochs, " << nDD << " DDs." << endl;
   if(CI.Screen)
      cout << "Data Totals: " << NEp << " epochs, " << nDD << " DDs." << endl;
}
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); }
}

//------------------------------------------------------------------------------------
double RMSResidualOfFit(int N, Vector<double>& dX, bool final)
{
try {
   int i,j,nd,cnt;
   double rms;
   string lab;
   Vector<double> f,Res;
   Matrix<double> P;
   map<DDid,DDData>::iterator it;
   format f166(16,6),f133(13,3),f82s(8,2,2);

      // open an output file for post fit DD residuals
   ofstream ddrofs;
   if(final && !CI.OutputDDRFile.empty()) {
      ddrofs.open(CI.OutputDDRFile.c_str(),ios::out);
      if(ddrofs) {
         oflog << "Opened file " << CI.OutputDDRFile
            << " for post fit residuals output." << endl;
         ddrofs << "# " << Title << endl;
         ddrofs << "RES site site sat sat week   sec_wk   count"
               << "            Data         Estimate         Residual" << endl;
      }
      else {
         oflog << "Warning - Failed to open DDR output file " << CI.OutputDDRFile
            << ". Do not output post fit residuals.\n";
      }
   }

      // go all the way back through the data
   nd= 0;
   rms = 0.0;
   cnt = -1;
   while(1) {
      cnt++;
      if(cnt > maxCount) break;
      Data = Vector<double>(Mmax,0.0);
      DataNL.clear();
      for(i=0,it=DDDataMap.begin(); it != DDDataMap.end(); it++) {
         j = index(it->second.count,cnt);
         if(j == -1) continue;
         if(CI.Frequency == 1) Data(i) = it->second.DDL1[j];
         if(CI.Frequency == 2) Data(i) = it->second.DDL2[j];
         if(CI.Frequency == 3)      // ionosphere-free phase
            Data(i) = if1p * it->second.DDL1[j] + if2p * it->second.DDL2[j];
         lab = ComposeName(it->first);
         DataNL += lab;
         i++;
      }
      if(i==0) continue;      // no data -- don't assume this is the end
      M = i;
      Data.resize(M);

      // this needed by EvaluateLSEquation
      SolutionEpoch = FirstEpoch + cnt*CI.DataInterval;
      EvaluateLSEquation(State,f,P);

      Res = Data - f;
      if(rms == 0.0) rms = norm(Res);
      else rms *= sqrt(1.0+norm(Res)/(rms*rms));
      nd += M;

      if(final && ddrofs) {
         string site1,site2;
         GSatID sat1,sat2;
         for(i=0; i<M; i++) {
            DecomposeName(DataNL.getName(i), site1, site2, sat1, sat2);
            ddrofs << "RES " << site1 << " " << site2 << " " << sat1 << " " << sat2
                  << " " << SolutionEpoch.printf("%4F %10.3g")
                  << " " << setw(5) << cnt
                  << " " << f166 << Data[i]
                  << " " << f166 << f[i]
                  << " " << f166 << Res[i]
                  << endl;
         }
      }

   }  // end loop over data

   if(final && ddrofs) ddrofs.close();

   rms /= sqrt(double(nd));

   return rms;
}
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); }
}

//------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------