disccorr.cpp

linux的gps应用

#pragma ident "$Id: DiscCorr.cpp 338 2006-12-07 21:40:55Z ehagen $"

//============================================================================
//
//  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
//
//============================================================================

/**
 * @file DiscCorr.cpp
 * GPS phase discontinuity correction. Given a SatPass object
 * containing dual-frequency pseudorange and phase for an entire satellite pass,
 * and a configuration object (as defined herein), detect discontinuities in
 * the phase and, if possible, estimate their size.
 * Output is in the form of Rinex editing commands (see class RinexEditor).
 */

#include <string>
#include <iostream>
#include <sstream>
#include <vector>
#include <deque>
#include <list>
#include <algorithm>

#include "StringUtils.hpp"
#include "Stats.hpp"
#include "PolyFit.hpp"
#include "icd_200_constants.hpp"    // PI,C_GPS_M,OSC_FREQ,L1_MULT,L2_MULT
#include "RobustStats.hpp"

#include "DiscCorr.hpp"

using namespace std;
using namespace gpstk;

// only to unclutter the top of this file; not included anywhere else...
#include "DCinternals.hpp"

//------------------------------------------------------------------------------------
// NB. string giving version of GDC (GDCVersion) is found in GDCconfiguration.cpp

// these are used only to associate a unique number in the log file with each pass
int GDCUnique=0;     // unique number for each call
int GDCUniqueFix;    // unique for each (WL,GF) fix

// conveniences only...
#define log *(p_oflog)
#define cfg(a) cfg_func(#a)

//------------------------------------------------------------------------------------
// These from SatPass.cpp
//const unsigned short SatPass::BAD = 0; // used by caller and DC to mark bad data
//const unsigned short SatPass::OK  = 1; // good data, no discontinuity
//const unsigned short SatPass::LL1 = 2; // good data, discontinuity on L1 only
//const unsigned short SatPass::LL2 = 4; // good data, discontinuity on L2 only
//const unsigned short SatPass::LL3 = 6; // good data, discontinuity on L1 and L2
const unsigned short GDCPass::WLDETECT =   2;
const unsigned short GDCPass::GFDETECT =   4;
const unsigned short GDCPass::DETECT   =   6;  // = WLDETECT | GFDETECT
const unsigned short GDCPass::WLFIX    =   8;
const unsigned short GDCPass::GFFIX    =  16;
const unsigned short GDCPass::FIX      =  24;  // = WLFIX | GFFIX

// notes on the use of these flags:
// if(flag & DETECT) is true for EITHER WL or GF or both
// if(flag & FIX)  is true for EITHER WL or GF or both
// if((flag & WLDETECT) && (flag & GFDETECT)) is true only for both WL and GF

// NB typical slip will have flag = DETECT+OK+FIX = 31
//    typical unfixed slip   flag = DETECT+OK     =  7

// BAD is used either as flag == BAD (for bad data) or flag != BAD (for good data),
// thus there are two gotcha's
//   - if a point is marked, but is later set BAD, that info is lost
//   - if a BAD point is marked, it becomes 'good'
// To avoid this we have to use OK rather than BAD:
// either !(flag & OK) or (flag ^ OK) for bad data, and (flag & OK) for good data

//------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------
// yes you need the gpstk::
int gpstk::DiscontinuityCorrector(SatPass& svp,
                                  GDCconfiguration& gdc,
                                  vector<string>& editCmds)
   throw(Exception)
{
try {
   int iret;
   GDCUnique++;

   // create the GDCPass from the input SatPass and the input GDC configuration
   GDCPass gp(svp,gdc);
   gp.initialize();

   // NB search for 'change the arrays' for places where arrays are re-defined
   // NB search for 'change the data' for places where the data is modified (! biases)
   // NB search for 'change the bias' for places where the bias is changed
   for(;;) {      // a convenience...
      // preparation
      if( (iret = gp.preprocess() )) break;
      if( (iret = gp.linearCombinations() )) break;

      // WL
      if( (iret = gp.detectWLslips() )) break;
      if( (iret = gp.fixAllSlips("WL") )) break;

      // GF
      if( (iret = gp.prepareGFdata() )) break;
      if( (iret = gp.detectGFslips() )) break;
      if( (iret = gp.WLconsistencyCheck() )) break;
      if( (iret = gp.fixAllSlips("GF") )) break;

      break;      // mandatory
   }

   // generate editing commands for deleted (flagged) data and slips,
   // use editing command (slips and deletes) to modify the original SatPass data
   // and print ending summary
   gp.finish(iret, svp, editCmds);

   // clear the temp arrays
   gp.clearTempArrays();

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

//------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------
int GDCPass::preprocess(void) throw(Exception)
{
try {
   int i,ilast,ngood;
   double biasL1,biasL2,dbias;
   list<Segment>::iterator it;

   if(cfg(Debug) >= 2) {
      DayTime CurrentTime;
      CurrentTime.setLocalTime();
      log << "\n======== Beg GPSTK Discontinuity Corrector " << GDCUnique
         << " ================================================\n";
      log << "GPSTK Discontinuity Corrector Ver. " << GDCVersion << " Run "
         << CurrentTime << endl;
   }

      // check input
   if(cfg(DT) <= 0) {
      log << "Error: data time interval is not set...Abort" << endl;
      return FatalProblem;
   }

      // create the first segment
   SegList.clear();
   {
      Segment s;
      s.nseg = 1;
      SegList.push_back(s);
   }
   it = SegList.begin();

      // loop over points in the pass
      // editing obviously bad data and adding segments where necessary
   for(ilast=-1,i=0; i<data.size(); i++) {

      if(!(data[i].flag & OK)) continue;

      // just in case the caller has set it to something else...
      data[i].flag = OK;

         // look for obvious outliers
         // Don't do this - sometimes the pseudoranges get extreme values b/c the
         // clock is allowed to run off for long times - perfectly normal
      //if(data[i].P1 < cfg(MinRange) || data[i].P1 > cfg(MaxRange) ||
      //   data[i].P2 < cfg(MinRange) || data[i].P2 > cfg(MaxRange) )
      //{
      //   data[i].flag = BAD;
      //   learn["points deleted: obvious outlier"]++;
      //   if(cfg(Debug) > 6)
      //      log << "Obvious outlier " << GDCUnique << " " << sat
      //         << " at " << i << " " << time(i).printf(outFormat) << endl;
      //   continue;
      //}

         // note first good point
      if(ilast == -1) ilast = it->nbeg = i;

         // is there a gap here? if yes, create a new segment
         // TD? do this here? why not allow any gap in the WL, and look for gaps
         // TD? only in the GF?
      if(cfg(DT)*(i-ilast) > cfg(MaxGap))
         it = createSegment(it,i,"initial gap");

         // count good points
      it->npts++;
      ilast = i;
   }

      // note last good point
   if(ilast == -1) ilast = it->nbeg;
   it->nend = ilast;

      // 'change the arrays' A1, A2 to be range minus phase for output
      // do the same at the end ("AFT")
      // loop over segments, counting the number of non-trivial ones
   for(ngood=0,it=SegList.begin(); it != SegList.end(); it++) {
      biasL1 = biasL2 = 0.0;

         // loop over points in this segment
      for(i=it->nbeg; i<=it->nend; i++) {
         if(!(data[i].flag & OK)) continue;

         dbias = fabs(data[i].P1-wl1*data[i].L1-biasL1);
         if(dbias > cfg(RawBiasLimit)) {
            if(cfg(Debug) >= 2) log << "BEFresetL1 " << GDCUnique
               << " " << sat << " " << time(i).printf(outFormat)
               << " " << fixed << setprecision(3) << biasL1
               << " " << data[i].P1 - wl1 * data[i].L1 << endl;
            biasL1 = data[i].P1 - wl1 * data[i].L1;
         }

         dbias = fabs(data[i].P2-wl2*data[i].L2-biasL2);
         if(dbias > cfg(RawBiasLimit)) {
            if(cfg(Debug) >= 2) log << "BEFresetL2 " << GDCUnique
               << " " << sat << " " << time(i).printf(outFormat)
               << " " << fixed << setprecision(3) << biasL2
               << " " << data[i].P2 - wl2 * data[i].L2 << endl;
            biasL2 = data[i].P2 - wl2 * data[i].L2;
         }

         A1[i] = data[i].P1 - wl1 * data[i].L1 - biasL1;
         A2[i] = data[i].P2 - wl2 * data[i].L2 - biasL2;

      }  // end loop over points in the segment

         // delete small segments
      if(it->npts < int(cfg(MinPts)))
         deleteSegment(it,"insufficient data in segment");
      else
         ngood++;
   }

   if(cfg(Debug) >= 2) dumpSegments("BEF",2,true);

   if(ngood == 0) return NoData;
   return ReturnOK;
}
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); }
}

//------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------
int GDCPass::linearCombinations(void) throw(Exception)
{
try {
   int i;
   double wlr,wlp,wlbias,gfr,gfp;
   list<Segment>::iterator it;

   // iterate over segments
   for(it=SegList.begin(); it != SegList.end(); it++) {
      it->npts = 0;                       // re-compute npts here

      // loop over points in this segment
      for(i=it->nbeg; i<=it->nend; i++) {
         if(!(data[i].flag & OK)) continue;

         wlr = wl1r * data[i].P1 + wl2r * data[i].P2;    // narrow lane range (m)
         wlp = wl1p * data[i].L1 + wl2p * data[i].L2;    // wide lane phase (m)
         gfr =        data[i].P1 -        data[i].P2;    // geometry-free range (m)
         gfp = gf1p * data[i].L1 + gf2p * data[i].L2;    // geometry-free phase (m)
         wlbias = (wlp-wlr)/wlwl;                        // wide lane bias (cycles)

         // change the bias
         if(it->npts == 0) {                             // first good point
            it->bias1 = wlbias;                          // WL bias (NWL)
            it->bias2 = gfp;                             // GFP bias
         }

         // change the arrays
         //data[i].L1 = unused!
         data[i].L2 = gfp;
         data[i].P1 = wlbias;
         data[i].P2 = -gfr;

         it->npts++;
      }
   }

   if(cfg(Debug) >= 2) dumpSegments("LCD");

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

//------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------
// detect slips in the wide lane bias
int GDCPass::detectWLslips(void) throw(Exception)
{
try {
   int iret;
   list<Segment>::iterator it;

   // look for obvious slips. this will break one segment into many
   if( (iret = detectObviousSlips("WL"))) return iret;

   for(it=SegList.begin(); it != SegList.end(); it++) {

      // compute stats and delete segments that are too small
      WLcomputeStats(it);

      // sigma-strip the WL bias, and remove small segments
      if(it->npts > 0) WLsigmaStrip(it);

      // print this before deleting segments with large sigma
      if(cfg(Debug) >= 1 && it->npts >= int(cfg(MinPts)))
         log << "WLSIG " << GDCUnique << " " << sat
            << " " << it->nseg
            << " " << time(it->nbeg).printf(outFormat)
            << fixed << setprecision(3)
            << " " << it->WLStats.StdDev()
            << " " << it->WLStats.Average()
            << " " << it->WLStats.Minimum()
            << " " << it->WLStats.Maximum()
            << " " << it->npts
            << " " << it->nbeg << " - " << it->nend
            << " " << it->bias1
            << " " << it->bias2
            << endl;

      // delete segments if sigma is too high...
      if(it->WLStats.StdDev() > cfg(WLNSigmaDelete)*cfg(WLSigma))
         deleteSegment(it,"WL sigma too big");

      // if there are less than about 2.5*cfg(WLWindowWidth) good points, don't bother
      // to use the sliding window method to look for slips; otherwise
      // compute stats for each segment using the 'two-paned sliding stats window',
      // store results in the temporary arrays
      if(double(it->npts) >= cfg(WLNWindows) * int(cfg(WLWindowWidth))) {
         iret = WLstatSweep(it,int(cfg(WLWindowWidth)));
         if(iret) return iret;
      }

   }  // end loop over segments

   // use the temporary arrays filled by WLstatSweep to detect slips in the WL bias