estimation.cpp

linux的gps应用

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() - inside the iteration loop
// currently, n is not used...
// currently does nothing but print stats on the residuals
int EditDDdata(int n)
{
try {
   int i,k;
   double res,median,mad,mest;
   map<DDid,DDData>::iterator it;

   oflog << "    Estimation data summary  "
         << "N   M-est    MAD     Ave     Std    SigYX  Slop_um SigSl_um" << endl;

      // loop over the data
   for(k=1, it = DDDataMap.begin(); it != DDDataMap.end(); it++) {
      vector<double> ddres,weights;
      TwoSampleStats<double> tsstats;

      for(i=0; i<it->second.count.size(); i++) {
         res = (CI.Frequency == 1 ? it->second.DDL1[i] - it->second.DDER[i] :
               (CI.Frequency == 2 ? it->second.DDL2[i] - it->second.DDER[i] :
                                    if1p*it->second.DDL1[i]+if2p*it->second.DDL2[i]
                                       - it->second.DDER[i]));
         tsstats.Add(it->second.count[i],res);
         ddres.push_back(res);
      }

      weights.resize(ddres.size());
      mad = Robust::MedianAbsoluteDeviation(&ddres[0], ddres.size(), median);
      mest = Robust::MEstimate(&ddres[0], ddres.size(), median, mad, &weights[0]);

         // output
      oflog << "EDS " << setw(2) << k << " " << it->first
         << " " << setw(5) << it->second.count.size()
         << fixed << setprecision(3)
         << " " << setw(7) << mest
         << " " << setw(7) << mad
         << " " << setw(7) << tsstats.AverageY()
         << " " << setw(7) << tsstats.StdDevY()
         << " " << setw(7) << tsstats.SigmaYX()
         << " " << setw(7) << tsstats.Slope()*1000000.0
         << " " << setw(7) << tsstats.SigmaSlope()*1000000.0
         << " " << setw(7) << tsstats.Slope()*1000.0*it->second.count.size()
         << endl;

      k++;
   }

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

//------------------------------------------------------------------------------------
// called by Estimation() - inside the iteration loop
int ModifyState(int niter)
{
try {
   int i,j,k;

      // set the State elements to zero for next iteration
   map<string,Station>::const_iterator it;
   for(it=Stations.begin(); it != Stations.end(); it++) {

      // ignore fixed sites
      if(it->second.fixed) continue;

      // find the position states
      i = StateNL.index(it->first+string("-X"));
      j = StateNL.index(it->first+string("-Y"));
      k = StateNL.index(it->first+string("-Z"));
      if(i == -1 || j == -1 || k == -1) {
         Exception e("Position states confused: unable to find for " + it->first);
         GPSTK_THROW(e);
      }

      State(i) = State(j) = State(k) = 0.0;
   }

      // ----------------- fix biases?
   if(Biasfix) {
      if(CI.Verbose) oflog << "Fix the biases:\n";
         // State must have the (fixed) biases still in it
      for(i=0; i<State.size(); i++) {
         string site1,site2;
         GSatID sat1,sat2;
         DecomposeName(StateNL.getName(i), site1, site2, sat1, sat2);
         if(site2 == string("X") || site2 == string("Y") || site2 == string("Z"))
            continue;
         if(site2 == string("rzd"))
            continue;
         if(Stations.find(site2) == Stations.end())
            continue;
         if(sat1.id == -1 || sat2.id == -2)
            continue;

         long bias=long(State[i]/wave + (State[i]/wave > 0 ? 0.5 : -0.5));
         if(CI.Verbose) oflog << "  fix " << StateNL.getName(i)
                              << " to " << bias << " cycles" << endl;
         State(i) = wave*double(bias);
      }
   }

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

//------------------------------------------------------------------------------------
// called by Estimation() - inside the iteration loop
// actually fixes the biases
int InitializeEstimator(void)
{
try {
   int i;
   Namelist NL;

      // ----------------- initialize this iteration
      // determine length of state, reset the LS
      // Use N and NL rather than NState and StateNL
   N = NState;
   NL = StateNL;
   if(Biasfix) {
      // State will still be full length = NState
      // StateNL will also stay full length, but N and NL will not
      NL.clear();
      for(N=0,i=0; i<NState; i++) {
         string site1,site2;
         GSatID sat1,sat2;
         DecomposeName(StateNL.getName(i), site1, site2, sat1, sat2);
         if(Stations.find(site2) != Stations.end() &&
               sat1.id != -1 &&
               sat2.id != -1)
            break;     // quit when first bias state found
         else {                    // not a bias state
            NL += StateNL.getName(i);
            N++;
         }
      }
      oflog << "Fix biases on this iteration (new State dimension is "
         << N << ")" << endl;
      if(CI.Screen) cout << "Fix biases on this iteration (new State dimension is "
         << N << ")" << endl;
   }
   dX.resize(N);
   srif = SRIFilter(NL);

      // save the nominal state for output with Solution (OutputIterationResults)
   NominalState = State;

      // dump the nominal state (including biases, even if fixed)
   //if(CI.Verbose) {
   //   LabelledVector LabSt(StateNL,State);
   //   LabSt.setw(20).setprecision(6);
   //   oflog << "Nominal State :\n" << LabSt << endl;
   //}

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

//------------------------------------------------------------------------------------
// called by Estimation() - inside the iteration loop
int aPrioriConstraints(void)
{
try {
      // add initial constraints
   Matrix<double> apCov(N,N,0.0);
   Vector<double> apState(N,0.0);         // most states have apriori value = 0

   int i,j,k,n;
   double ss;
   Position BL;
   map<string,Station>::const_iterator it;

      // set apCov = unity
   //ident(apCov);

      // loop over baselines - to get the position constraints
   for(n=0; n<Baselines.size(); n++) {
      string one=StringUtils::word(Baselines[n],0,'-');
      string two=StringUtils::word(Baselines[n],1,'-');
      BL = Stations[one].pos - Stations[two].pos;

         // find the position states
      i = StateNL.index(two+string("-X"));
      j = StateNL.index(two+string("-Y"));
      k = StateNL.index(two+string("-Z"));
         // you may have a baseline where both sites are fixed.
      if(i == -1 || j == -1 || k == -1) continue;

      if(Biasfix) {     // 10ppm of baseline
         ss = CI.TightConstraint * fabs(BL.X());
         apCov(i,i) = (ss*ss);
         ss = CI.TightConstraint * fabs(BL.Y());
         apCov(j,j) = (ss*ss);
         ss = CI.TightConstraint * fabs(BL.Z());
         apCov(k,k) = (ss*ss);
         // 1 mm v2.6
         //ss = 1.e-3 ;
         //apCov(i,i) = (ss*ss);
         //ss = 1.e-3 ;
         //apCov(j,j) = (ss*ss);
         //ss = 1.e-3 ;
         //apCov(k,k) = (ss*ss);
      }
      else {            // loose on position
         ss = CI.LooseConstraint * fabs(BL.X());
         apCov(i,i) = (ss*ss);
         ss = CI.LooseConstraint * fabs(BL.Y());
         apCov(j,j) = (ss*ss);
         ss = CI.LooseConstraint * fabs(BL.Z());
         apCov(k,k) = (ss*ss);
      }

      if(CI.Verbose) {
         // assume i,j,k are in order:
         MatrixSlice<double> Rslice(apCov,i,i,3,3);
         Matrix<double> R(Rslice);
         Namelist NL;
         NL += StateNL.getName(i);
         NL += StateNL.getName(j);
         NL += StateNL.getName(k);
         LabelledMatrix Lapc(NL,R);
         Lapc.setw(20).setprecision(3).scientific();
         Lapc.message("a priori covariance");
         oflog << Lapc << endl;
      }
   }

      // constrain the residual trop delay
   if(CI.NRZDintervals > 0) {
         // RZD in different intervals correlated; first order Gauss-Markov
         // dt = time between intervals in hours; these unused if N==1
      double dt = (LastEpoch-FirstEpoch)/(3600.*CI.NRZDintervals);
      double exn,ex = exp(-dt/CI.RZDtimeconst);

         // do for each site
      for(it=Stations.begin(); it != Stations.end(); it++) {

            // find indexes in state vector of all RZD states for this site
         string stname;
         vector<int> indexes;
         for(n=0; n<CI.NRZDintervals; n++) {
            stname = it->first + string("-RZD") + StringUtils::asString(n);
            i = StateNL.index(stname);
            if(i == -1) {
               Exception e("RZD states confused: unable to find state " + stname);
               GPSTK_THROW(e);
            }
            if(CI.Debug) oflog << "RZD state " << stname << " = index " << i << endl;
            indexes.push_back(i);
         }

            // fill the matrix
         for(n=0; n<indexes.size(); n++) {
               // diagonal element
            i = indexes[n];
            apCov(i,i) = CI.RZDsigma * CI.RZDsigma;
               // off-diagonal elements: rows up and cols to the left
            exn = ex;
            for(k=n-1; k>=0; k--) {
               j = indexes[k];
               apCov(j,i) = apCov(i,j) = CI.RZDsigma * CI.RZDsigma * exn;
               exn *= ex;
            }
         }  // end loop over diagonal matrix elements for this site

            // dump
         if(CI.Verbose) {
            if(CI.NRZDintervals > 1) {
               // assume indexes are in order:
               MatrixSlice<double> Rslice(apCov,indexes[0],indexes[0],
                                          CI.NRZDintervals,CI.NRZDintervals);
               Matrix<double> R(Rslice);
               Namelist NL;
               for(n=0; n<indexes.size(); n++) NL += StateNL.getName(indexes[n]);
               LabelledMatrix Lapc(NL,R);
               Lapc.setw(20).setprecision(3).scientific();
               Lapc.message("a priori covariance");
               oflog << Lapc << endl;
            }
            else
               oflog << "a priori covariance for RZD at " << it->first
                  << ": " << setprecision(3) << scientific << CI.RZDsigma*CI.RZDsigma
                  << endl;
         }

      }  // end loop over sites

   }  // end if there are RZD states..

      // TD need to constrain biases ... what is reasonable?
   if(!Biasfix) {
      ss = 0.25 * wave;
      for(n=0; n<StateNL.size(); n++) {
         string site1,site2;
         GSatID sat1,sat2;
         DecomposeName(StateNL.getName(n), site1, site2, sat1, sat2);
         if(site2 == string("X") || site2 == string("Y") || site2 == string("Z")) {
            continue;   // oflog << " : " << site2 << "-component position";
         }
         else if(site2.substr(0,3) == "RZD") {
            continue;   // oflog << " : trop #" << site2.substr(3,site2.size()-3);
         }
         else if(Stations.find(site2) != Stations.end() &&
               sat1.id != -1 &&
               sat2.id != -1) {
            // bias oflog << " " << site2 << " " << sat1 << " " << sat2 << " : bias";
            apCov(n,n) = ss*ss;
         }
         else
            continue;   // oflog << " : unknown!";
      }
      oflog << "a priori covariance for biases : " << setprecision(3)
         << scientific << ss*ss << endl;
   }

      // dump the whole matrix
   //if(CI.Verbose) {
   //   LabelledMatrix Lapc(StateNL,apCov);
   //   Lapc.setw(20).setprecision(3).scientific();
   //   Lapc.message("a priori covariance");
   //   oflog << Lapc << endl;
   //}

      // add it to srif
   srif.addAPriori(apCov,apState);

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

//------------------------------------------------------------------------------------
// called by Estimation() - inside the data loop, inside the iteration loop
int FillDataVector(int count)
{
try {
   int i,j;
   string lab;

   Data = Vector<double>(Mmax,0.0);
   DataNL.clear();
      // loop over the data
   map<DDid,DDData>::iterator it;
   for(i=0,it = DDDataMap.begin(); it != DDDataMap.end(); it++) {
      j = index(it->second.count,count);
      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) {
      Data.resize(i);
      if(CI.Debug) {
         LabelledVector LD(DataNL,Data);
         LD.setw(20).setprecision(6);
         oflog << "At count " << count
            << " found time " << SolutionEpoch.printf("%F %10.3g")
            << " and Data\n" << LD << endl;
      }
   }

   return i;
}
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() - inside the data loop, inside the iteration loop
// Input is Namelist DNL, the double difference data Namelist (DataNL)
// Output is MCov, the measurement covariance matrix for this data (MeasCov).
// Let:
//  d = vector of one-way data (one site, one satellite)
// sd = vector of single difference data (two sites, one satellite)
// dd = vector of double difference data (two sites, two satellites)
// DD and SD are matricies with elements 0,1,-1 which transform d to sd to dd:
// sd = SD * d
// dd = DD * sd