synchronization.cpp

linux的gps应用

#pragma ident "$Id: Synchronization.cpp 185 2006-10-05 18:21:39Z btolman $"

//============================================================================
//
//  This file is part of GPSTk, the GPS Toolkit.
//
//  The GPSTk is free software; you can redistribute it and/or modify
//  it under the terms of the GNU Lesser General Public License as published
//  by the Free Software Foundation; either version 2.1 of the License, or
//  any later version.
//
//  The GPSTk is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU Lesser General Public License for more details.
//
//  You should have received a copy of the GNU Lesser General Public
//  License along with GPSTk; if not, write to the Free Software Foundation,
//  Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
//  
//  Copyright 2004, The University of Texas at Austin
//
//============================================================================

//============================================================================
//
//This software developed by Applied Research Laboratories at the University of
//Texas at Austin, under contract to an agency or agencies within the U.S. 
//Department of Defense. The U.S. Government retains all rights to use,
//duplicate, distribute, disclose, or release this software. 
//
//Pursuant to DoD Directive 523024 
//
// DISTRIBUTION STATEMENT A: This software has been approved for public 
//                           release, distribution is unlimited.
//
//=============================================================================

/**
 * @file Synchronization.cpp
 * Interpolate the phase data to correct for the clock offset, synchronizing the
 * data at different stations; part of program DDBase.
 */

//------------------------------------------------------------------------------------
// TD Synchronization.cpp make number of phase points in fit an input parameter
// TD Synchronization.cpp make MaxGap=10; an input parameter and
// TD Synchronization.cpp use this in EditRawDataBuffers() to remove single points
// TD Synchronization.cpp   that have gaps larger than this on each side of them.

//------------------------------------------------------------------------------------
// includes
// system
#include <deque>

// GPSTk
#include "geometry.hpp"             // DEG_TO_RAD
#include "PolyFit.hpp"
#include "EphemerisRange.hpp"

// DDBase
#include "DDBase.hpp"
#include "PhaseWindup.hpp"
#include "index.hpp"

//------------------------------------------------------------------------------------
using namespace std;
using namespace gpstk;

//------------------------------------------------------------------------------------
// prototypes -- this module only -- called by Synchronization()
void FitPhaseAndMoveData(GSatID& sat, string site, Station& st, RawData& rd);

//------------------------------------------------------------------------------------
int Synchronization(void)
{
try {
   if(CI.Verbose) oflog << "BEGIN Synchronization()" << endl;

   GSatID sat;
   map<string,Station>::iterator it;
   map<GSatID,RawData>::iterator jt;

      // loop over stations
   for(it=Stations.begin(); it != Stations.end(); it++) {
      //string label = it->first;
      Station& st=it->second;
         // loop over satellites
      for(jt=st.RawDataBuffers.begin(); jt != st.RawDataBuffers.end(); jt++) {
         sat = jt->first;
         RawData& rawdat=jt->second;

         if(rawdat.count.size() == 0) continue;

            // Loop over all points in the buffers, using a sliding window.
            // For each window, fit a polynomial to the phase data.
            // At each point, evaluate the polynomial at the true receive time.
         FitPhaseAndMoveData(sat,it->first,st,rawdat);

      }  // loop over sats

   }  // loop over stations
   
   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); }
}   // end Synchronization()

//------------------------------------------------------------------------------------
// Process using a sliding window:
// loop over all points in the buffers of RawData, using a sliding window of fixed
// length which is centered (as much as possible) about the buffer point of interest.
// Process each buffer point using the data in the sliding window.
void FitPhaseAndMoveData(GSatID& sat, string site, Station& statn, RawData& rawdat)
{
try {
   const int N=11;   // size of the window // best odd  // TD make input
   const int D=3;    // degree of polynomial to be fit   // TD make input
   bool change;      // mark a change in the deques --> fit a new polynomial
   int nc;           // index into the buffer at the current point
   int nbeg;         // index into the buffer at the start of the window
   int nend;         // index into the buffer at the end of the window
   int nhalf=N/2;    // half the window size
   int len;          // length of the buffers
   int ngap;         // number of counts between the end pt (nend) and the next
   int nsize;        // size of the sliding window (deques)
   int i,j;
   double x,x0,d10,d20,dx,dph1,dph2;
   PolyFit<double> PF1,PF2;// fit polynomials to L1 and L2 phase
   deque<int> dc;          // the sliding window : time -- keep the deques 
   deque<double> d1,d2;    // the sliding window : data -- parallel

      // starting: nend is before the current point (0)
   nbeg = 0;
   nend = -1;
   change = true;
   len = int(rawdat.count.size());  // length of the buffers

      // Loop over count (epochs). At each count, fill a 'sliding window' deque
      // (one for each of count, L1 and L2) with up to N points, including
      // containing the current count. The points run from index nbeg to nend.
   for(nc=0; nc<len; nc++) {
         // -------------------------------------------------------------
         // the only way this could be true is if the current point is the
         // first point past a big (>=MaxGap) gap
//if(CI.Verbose && site=="Aref" && sat.id==29) oflog << "LOOP nc=" << nc << " nbeg=" << nbeg << " nend=" << nend << " ngap=" << (rawdat.count[nend+1]-rawdat.count[nend]) << " len=" << len << endl;
      if(nc > nend) {
            // clear window and start again
         dc.clear();
         d1.clear();
         d2.clear();
         nbeg = nend = nc;
         ngap = rawdat.count[nend+1]-rawdat.count[nend];
         if(ngap >= CI.MaxGap) continue;        // skip this point if there's a gap
         dc.push_back(rawdat.count[nend]);      // time / DataInterval
         d1.push_back(rawdat.L1[nend]);         // cycles
         d2.push_back(rawdat.L2[nend]);         // cycles
         change = true;
//if(CI.Verbose && site=="Aref" && sat.id==29) oflog << "clear" << endl;
      }

         // -------------------------------------------------------------
         // advance the end of the window (nend) when all these are true:
      while(   (nend < len-1)       // point is not beyond the end of the buffer
            && (nend-nbeg+1 < N)    // & the window is not full
                                    // & there is not a big gap
            && ((ngap = rawdat.count[nend+1]-rawdat.count[nend]) < CI.MaxGap)
            && (nc >= nbeg)         // & the current point will stay in window
            ) {
            // expand the window one point into the future
         nend++;
         dc.push_back(rawdat.count[nend]);      // keep the deques parallel
         d1.push_back(rawdat.L1[nend]);
         d2.push_back(rawdat.L2[nend]);
         change = true;
//if(CI.Verbose && site=="Aref" && sat.id==29) oflog << "advance" << endl;
      };

         // -------------------------------------------------------------
         // is this an isolated point?
      //if(ngap >= CI.MaxGap) {
      //   nc = nend+1;
      //   continue;
      //}

         // -------------------------------------------------------------
         // Process the deques when a change has been made
      if(change) {
//if(CI.Verbose && site=="Aref" && sat.id==29) {
//oflog << "buffer:";
//for(i=nbeg; i<=nend; i++) oflog << " " << rawdat.count[i];
//oflog << " end+1: " << rawdat.count[nend+1] << endl;
//}
            // size of the sliding window (deques)
         nsize = dc.size();

            // must not have isolated points
            // EditRawBuffers should have removed these
         if(nsize < 2) {
            Exception e( (nsize == 0 ?
               string("ERROR - empty window") : string("ERROR - isolated point"))
               + string(" for station ") + site + string(" and satellite ")
               + sat.toString()