estimation.cpp

linux的gps应用

// dd = DD * SD * d
// The covariance matrix will be MC = (DD*SD)*transpose(DD*SD)
//                                  = DD*SD*transpose(SD)*transpose(DD)
// If the one-way data has a measurement covariance, then fill the vector d with
// them; then MC = DD*SD* d * transpose(SD)*transpose(DD).
// Building DD and SD is just a matter of lists:
// loop through the dd namelist, keeping lists of:
// one-way data (site-satellite pairs) (d)
// single differences (site-site-satellite sets) (sd)
// and you have a list of double differences (DNL)
//
// TD MinElevation here should be a separate parameter, not necessarily the mask angle
void StochasticModel(int count, Namelist& DNL, Matrix<double>& MCov)
{
try {
   unsigned int m=DNL.size();
   if(m==0) return;

   int i,j,in,jn,kn;
   double eps,coselev,d0,sig0;
   string site1,site2;
   GSatID sat1,sat2;
   vector<double> d;    // weights of one-way data
   vector<OWid> ld;     // labels of d
   vector<SDid> sd;

   // eps non-zero avoids blowup at zenith
   //eps = 0.0000001;   seems to have no effect
   eps = 0.001;

   for(i=0; i<DNL.size(); i++) {
      // break the label into site1,site2,sat1,sat2
      DecomposeName(DNL.getName(i), site1, site2, sat1, sat2);
      if(index(ld,OWid(site1,sat1)) == -1) ld.push_back(OWid(site1,sat1));
      if(index(ld,OWid(site1,sat2)) == -1) ld.push_back(OWid(site1,sat2));
      if(index(ld,OWid(site2,sat1)) == -1) ld.push_back(OWid(site2,sat1));
      if(index(ld,OWid(site2,sat2)) == -1) ld.push_back(OWid(site2,sat2));
      if(index(sd,SDid(site1,site2,sat1)) == -1) sd.push_back(SDid(site1,site2,sat1));
      if(index(sd,SDid(site1,site2,sat2)) == -1) sd.push_back(SDid(site1,site2,sat2));
   }

      // fill d with the weights
      // d needs to have units meters and be realistic ~ sigma(phase)
      // =sig0(m) at min elev, smaller at higher elevation
   sig0 = 1.0e-3; // smaller than e-2, else little effect
   coselev = eps + cos(CI.MinElevation * DEG_TO_RAD);          // TD new input param
   d0 = sig0 / (coselev*coselev);   // cosine squared model
   //d0 = sig0 / coselev;            // cosine model
   d = vector<double>(ld.size());
   for(i=0; i<ld.size(); i++) {
      j = index(Stations[ld[i].site].RawDataBuffers[ld[i].sat].count,count);
      if(j == -1) {
         oflog << "Error -- count " << count << " not found in buffer for " << ld[i]
            << endl;
         d[i] = d0;
      }
      else {
         coselev = eps + cos(Stations[ld[i].site].RawDataBuffers[ld[i].sat].elev[j]
                           * DEG_TO_RAD);
         d[i] = d0 * coselev * coselev;   // cosine squared model
         //d[i] = d0 * coselev;            // cosine model
      }
   }

   // temp
   //format f113s(11,3,2);
   //oflog << "DDs are (" << DNL.size() << "):\n" << setw(20) << DNL << endl;
   //oflog << "SDs are: (" << sd.size() << ")" << fixed << endl;
   //for(i=0; i<sd.size(); i++) oflog << " / " << sd[i];
   //oflog << endl;
   //oflog << "OWs are: (" << ld.size() << ")" << endl;
   //for(i=0; i<ld.size(); i++) oflog << " / " << ld[i];
   //oflog << endl;
   //oflog << "OW wts are: (" << d.size() << ")" << endl;
   //for(i=0; i<d.size(); i++) oflog << " " << f113s << d[i];
   //oflog << endl;

   Matrix<double> SD(sd.size(),ld.size(),0.0);
   Matrix<double> DD(m,sd.size(),0.0);
   // TD need to account for signs here ... sd[.] may be site2,site1,sat1 ...
   for(in=0; in<DNL.size(); in++) {
      DecomposeName(DNL.getName(in), site1, site2, sat1, sat2);
      jn = index(sd,SDid(site1,site2,sat1));        // site1-site2, sat1
      DD(in,jn) = 1;
      kn = index(ld,OWid(site1,sat1));              // site1, sat1
      SD(jn,kn) = d[kn];
      kn = index(ld,OWid(site2,sat1));              // site2, sat1
      SD(jn,kn) = -d[kn];

      jn = index(sd,SDid(site1,site2,sat2));        // site1-site2, sat2
      DD(in,jn) = -1;
      kn = index(ld,OWid(site1,sat2));              // site1, sat2
      SD(jn,kn) = d[kn];
      kn = index(ld,OWid(site2,sat2));              // site2, sat2
      SD(jn,kn) = -d[kn];
   }

   //oflog << " SD is\n" << fixed << setw(3) << SD << endl;
   //oflog << " DD is\n" << fixed << setw(3) << DD << endl;

   Matrix<double> T;
   T = DD * SD;
   MCov = T * transpose(T);

   if(CI.Debug) oflog << "Measurement covariance is\n" << scientific
      << setw(8) << setprecision(3) << MeasCov << 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() - inside the data loop, inside the iteration loop
// Given a nominal state vector X, compute the function f(X) and the partials matrix
// P at X.
// NB X is not used here ... except that State is used for biases
void EvaluateLSEquation(Vector<double>& X, Vector<double>& f, Matrix<double>& P)
{
try {
   int i,j,k,n,m,ntrop;
   double ER,trop,mapf;
   string site1,site2;
   GSatID sat1,sat2;
   PreciseRange CER;
   //CorrectedEphemerisRange CER;

      //
      // assume Station.pos has been defined outside this routine... in UpdateNom.St.
      //

      // find the trop estimation interval for this epoch
      // trop is a temporary here..
   if(CI.NRZDintervals > 0) {
      ntrop = int( (SolutionEpoch-FirstEpoch) /
                    (((LastEpoch-FirstEpoch)+CI.DataInterval)/CI.NRZDintervals) );
   }

      // loop over the data vector, computing f(X) and filling P
   f = Vector<double>(M,0.0);
   P = Matrix<double>(M,N,0.0);
   for(m=0; m<DataNL.size(); m++) {

         // break name into its parts
      DecomposeName(DataNL.getName(m), site1, site2, sat1, sat2);

      Station& st1=Stations[site1];
      Station& st2=Stations[site2];

         // -----------------------------------------------------------
         // site1 ----------------------------------------------------
      if(!st1.fixed) {
         i = StateNL.index(site1 + string("-X"));
         j = StateNL.index(site1 + string("-Y"));
         k = StateNL.index(site1 + string("-Z"));
         if(i == -1 || j == -1 || k == -1) {
            Exception e("Position states confused: unable to find for " + site1);
            GPSTK_THROW(e);
         }
      }
         // sat 1  CER.SVR is satellite Position
         // pos 1  st1.pos is station Position

         // position states
         // should you use CER.rawrange here?
      ER = CER.ComputeAtReceiveTime(SolutionEpoch,st1.pos,sat1.id,*pEph,eorient);
      trop = st1.pTropModel->correction(st1.pos,CER.SVR,SolutionEpoch);
      f(m) += ER+trop;
      if(!st1.fixed) {
         P(m,i) += CER.cosines[0];
         P(m,j) += CER.cosines[1];
         P(m,k) += CER.cosines[2];
      }

         // trop rzd .. depends on site, sat and trop model
      if(CI.NRZDintervals > 0) {
         n = StateNL.index(site1 + string("-RZD") + StringUtils::asString(ntrop));
         if(n == -1) {
            Exception e("RZD states confused: unable to find state " + 
               site1 + string("-RZD") + StringUtils::asString(ntrop));
            GPSTK_THROW(e);
         }
         mapf = st1.pTropModel->wet_mapping_function(CER.elevation);
         P(m,n) += mapf;
         f(m) += mapf * State(n);
      }

         // sat 2 -----------------------------------------------------
      ER = CER.ComputeAtReceiveTime(SolutionEpoch,st1.pos,sat2.id,*pEph,eorient);
      trop = st1.pTropModel->correction(st1.pos,CER.SVR,SolutionEpoch);
      f(m) -= ER+trop;
      if(!st1.fixed) {
         P(m,i) -= CER.cosines[0];
         P(m,j) -= CER.cosines[1];
         P(m,k) -= CER.cosines[2];
      }

         // trop rzd .. depends on site, sat and trop model
      if(CI.NRZDintervals > 0) {
         mapf = st1.pTropModel->wet_mapping_function(CER.elevation);
         P(m,n) += mapf;
         f(m) += mapf * State(n);
      }

         // -----------------------------------------------------------
         // site 2 ----------------------------------------------------
      if(!st2.fixed) {
         i = StateNL.index(site2 + string("-X"));
         j = StateNL.index(site2 + string("-Y"));
         k = StateNL.index(site2 + string("-Z"));
         if(i == -1 || j == -1 || k == -1) {
            Exception e("Position states confused: unable to find for " + site2);
            GPSTK_THROW(e);
         }
      }
         // sat 1 -----------------------------------------------------
      ER = CER.ComputeAtReceiveTime(SolutionEpoch,st2.pos,sat1.id,*pEph,eorient);
      trop = st2.pTropModel->correction(st2.pos,CER.SVR,SolutionEpoch);
      f(m) -= ER+trop;
      if(!st2.fixed) {
         P(m,i) -= CER.cosines[0];
         P(m,j) -= CER.cosines[1];
         P(m,k) -= CER.cosines[2];
      }

         // trop rzd .. depends on site, sat and trop model
      if(CI.NRZDintervals > 0) {
         n = StateNL.index(site2 + string("-RZD") + StringUtils::asString(ntrop));
         if(n == -1) {
            Exception e("RZD states confused: unable to find state " + 
               site2 + string("-RZD") + StringUtils::asString(ntrop));
            GPSTK_THROW(e);
         }
         mapf = st2.pTropModel->wet_mapping_function(CER.elevation);
         P(m,n) += mapf;
         f(m) += mapf * State(n);
      }

         // sat 2 -----------------------------------------------------
      ER = CER.ComputeAtReceiveTime(SolutionEpoch,st2.pos,sat2.id,*pEph,eorient);
      trop = st2.pTropModel->correction(st2.pos,CER.SVR,SolutionEpoch);
      f(m) += ER+trop;
      if(!st2.fixed) {
         P(m,i) += CER.cosines[0];
         P(m,j) += CER.cosines[1];
         P(m,k) += CER.cosines[2];
      }

         // trop rzd .. depends on site, sat and trop model
      if(CI.NRZDintervals > 0) {
         mapf = st2.pTropModel->wet_mapping_function(CER.elevation);
         P(m,n) += mapf;
         f(m) += mapf * State(n);
      }

         // -----------------------------------------------------------
         // bias ------------------------------------------------------
      j = 1;
      i = StateNL.index(DataNL.getName(m));
      if(i == -1) {
         // but what if the bias is A-B_s-r and the data B-A_r-s?
         // (Decompose was called at top of loop)
         j = -1;
         i = StateNL.index(ComposeName(site1,site2,sat2,sat1));      // most likely
         if(i == -1) {
            i = StateNL.index(ComposeName(site2,site1,sat1,sat2));
            if(i == -1) {
               j = 1;
               i = StateNL.index(ComposeName(site2,site1,sat2,sat1));
            }
         }
      }
      f(m) += j * State(i);
      if(!Biasfix)
         P(m,i) = j;

   }  // end loop over data

   f.resize(M);
   P.resize(M,N);

}
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 MeasurementUpdate(Matrix<double>& P, Vector<double>& f, Matrix<double>& MC)
{
try {

   srif.measurementUpdate(P,f,MC);

   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 Solve(void)
{
try {

   try {
      srif.getStateAndCovariance(dX,Cov,&small,&big);
   }
   catch(SingularMatrixException& sme) {
      oflog << "Problem is singular " << endl;
      return -2;                 // TD handle singular problems in Solve()
   }

   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 UpdateNominalState(void)
{
try {
   int n,i,j,k;

   if(Biasfix) {
      // NB when Biasfix, State has dimension NState buf dX has dimension N>NState
      // EvaluateLSFunction uses State bias elements even when Biasfix
      for(i=0; i<N; i++) {
         State[i] += dX[i];
      }
   }
   else {                  // regular update, save for when Biasfix is set
      State += dX;
      BiasState = State;
      BiasCov = Cov;
   }
      // redefine the nominal position
      // set all floating position states to zero
   map<string,Station>::const_iterator it;
   for(it=Stations.begin(); it != Stations.end(); it++) {
      if(it->second.fixed)       // ignore fixed sites
         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);
      }

      // update the nominal position in Stations[]
      Stations[it->first].pos.setECEF(
         Stations[it->first].pos.X()+dX(i),
         Stations[it->first].pos.Y()+dX(j),
         Stations[it->first].pos.Z()+dX(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
void OutputIterationResults(bool final)
{
try {
   int i,N=dX.size();
   format f166(16,6),f206(20,6),f82s(8,2,2);

   oflog << "         State label"
         << "    Nominal State"
         << "     State Update"
         << "     New Solution"
         << "            Sigma"
         << endl;
   for(i=0; i<N; i++) {
      oflog << setw(20) << StateNL.getName(i)
            << " " << f166 << NominalState[i]
            << " " << f166 << dX[i]
            << " " << f166 << State[i]
            << " " << f166 << SQRT(Cov(i,i))
            << endl;
   }

   //LabelledVector LSol(StateNL,dX);
   //LSol.setw(20).setprecision(6);
   //oflog << "Solution\n" << LSol << endl;

   ////LabelledMatrix LCov(StateNL,Cov);
   //Vector<double> Sig(N);
   //for(i=0; i<N; i++) Sig(i)=SQRT(Cov(i,i));