1 //==============================================================================
2 //
3 //  This file is part of GPSTk, the GPS Toolkit.
4 //
5 //  The GPSTk is free software; you can redistribute it and/or modify
6 //  it under the terms of the GNU Lesser General Public License as published
7 //  by the Free Software Foundation; either version 3.0 of the License, or
8 //  any later version.
9 //
10 //  The GPSTk is distributed in the hope that it will be useful,
11 //  but WITHOUT ANY WARRANTY; without even the implied warranty of
12 //  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 //  GNU Lesser General Public License for more details.
14 //
15 //  You should have received a copy of the GNU Lesser General Public
16 //  License along with GPSTk; if not, write to the Free Software Foundation,
17 //  Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
18 //
19 //  This software was developed by Applied Research Laboratories at the
20 //  University of Texas at Austin.
21 //  Copyright 2004-2020, The Board of Regents of The University of Texas System
22 //
23 //==============================================================================
24 
25 //==============================================================================
26 //
27 //  This software was developed by Applied Research Laboratories at the
28 //  University of Texas at Austin, under contract to an agency or agencies
29 //  within the U.S. Department of Defense. The U.S. Government retains all
30 //  rights to use, duplicate, distribute, disclose, or release this software.
31 //
32 //  Pursuant to DoD Directive 523024
33 //
34 //  DISTRIBUTION STATEMENT A: This software has been approved for public
35 //                            release, distribution is unlimited.
36 //
37 //==============================================================================
38 
39 /// @file DiscCorr.cpp GPS phase discontinuity correction. Given a SatPass object
40 /// containing dual-frequency pseudorange and phase for an entire satellite pass,
41 /// and a configuration object (as defined herein), detect discontinuities in
42 /// the phase and, if possible, estimate their size.
43 /// Output is in the form of Rinex editing commands (see class RinexEditor).
44 
45 //------------------------------------------------------------------------------------
46 // system
47 #include <string>
48 #include <iostream>
49 #include <sstream>
50 #include <vector>
51 #include <deque>
52 #include <list>
53 #include <algorithm>
54 // gpstk
55 #include "StringUtils.hpp"
56 #include "Stats.hpp"
57 #include "PolyFit.hpp"
58 #include "GNSSconstants.hpp"    // PI,C_MPS,OSC_FREQ_GPS,L1_MULT_GPS,L2_MULT_GPS
59 #include "RobustStats.hpp"
60 // geomatics
61 #include "DiscCorr.hpp"
62 
63 using namespace std;
64 using namespace gpstk;
65 using namespace StringUtils;
66 
67 //------------------------------------------------------------------------------------
68 // class GDCconfiguration
69 //------------------------------------------------------------------------------------
70 // class GDCconfiguration: string giving version of gpstk Discontinuity Corrector
71 const string GDCconfiguration::GDCVersion = string("6.3 12/15/2015");
72 
73 // class GDCconfiguration: member functions
74 //------------------------------------------------------------------------------------
75 // Set a parameter in the configuration; the input string 'cmd' is of the form
76 // '[--DC]<id><s><value>' : separator s is one of ':=,' and leading --DC is optional.
setParameter(string cmd)77 void GDCconfiguration::setParameter(string cmd)
78 {
79 try {
80    if(cmd.empty()) return;
81       // remove leading --DC
82    while(cmd[0] == '-') cmd.erase(0,1);
83    if(cmd.substr(0,2) == "DC") cmd.erase(0,2);
84 
85    string label, value;
86    string::size_type pos=cmd.find_first_of(",=:");
87    if(pos == string::npos) {
88       label = cmd;
89    }
90    else {
91       label = cmd.substr(0,pos);
92       value = cmd;
93       value.erase(0,pos+1);
94    }
95 
96    setParameter(label, asDouble(value));
97 }
98 catch(Exception& e) { GPSTK_RETHROW(e); }
99 catch(std::exception& e) {
100    Exception E("std except: "+string(e.what())); GPSTK_THROW(E);
101 }
102 catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
103 }
104 
105 //------------------------------------------------------------------------------------
106 // Set a parameter in the configuration using the label and the value,
107 // for booleans use (T,F)=(non-zero,zero).
setParameter(string label,double value)108 void GDCconfiguration::setParameter(string label, double value)
109 {
110 try {
111    if(CFG.find(label) == CFG.end())
112       ; // throw
113    else {
114       // log is not defined yet
115       if(CFG["Debug"] > 0) *(p_oflog) << "GDCconfiguration::setParameter sets "
116          << label << " to " << value << endl;
117       CFG[label] = value;
118    }
119 }
120 catch(Exception& e) { GPSTK_RETHROW(e); }
121 catch(std::exception& e) {
122    Exception E("std except: "+string(e.what())); GPSTK_THROW(E);
123 }
124 catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
125 }
126 
127 //------------------------------------------------------------------------------------
128 // Print help page, including descriptions and current values of all
129 // the parameters, to the ostream.
DisplayParameterUsage(ostream & os,bool advanced)130 void GDCconfiguration::DisplayParameterUsage(ostream& os, bool advanced)
131 {
132 try {
133    os << "GPSTk Discontinuity Corrector (GDC) v." << GDCVersion
134       << " configuration:"
135       //<< "\n  [ pass setParameter() a string '<label><sep><value>';"
136       //<< " <sep> is one of ,=: ]"
137       << endl;
138 
139    map<string,double>::const_iterator it;
140    for(it=CFG.begin(); it != CFG.end(); it++) {
141       if(CFGdescription[it->first][0] == '*')      // advanced options
142          continue;
143       ostringstream stst;
144       stst << it->first                            // label
145          << "=" << it->second;                     // value
146       os << " " << leftJustify(stst.str(),18)
147          << " : " << CFGdescription[it->first]     // description
148          << endl;
149    }
150    if(advanced) {
151    os << "   Advanced options:\n";
152    for(it=CFG.begin(); it != CFG.end(); it++) {
153       if(CFGdescription[it->first][0] != '*')      // ordinary options
154          continue;
155       ostringstream stst;
156       stst << it->first                            // label
157          << "=" << it->second;                     // value
158       os << " " << leftJustify(stst.str(),25)
159          << " : " << CFGdescription[it->first].substr(2)  // description
160          << endl;
161    }
162    }
163 }
164 catch(Exception& e) { GPSTK_RETHROW(e); }
165 catch(std::exception& e) {
166    Exception E("std except: "+string(e.what())); GPSTK_THROW(E);
167 }
168 catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
169 }
170 
171 
172 //------------------------------------------------------------------------------------
173 #define setcfg(a,b,c) { CFG[#a]=b; CFGdescription[#a]=c; }
174 // initialize with default values
initialize(void)175 void GDCconfiguration::initialize(void)
176 {
177 try {
178    p_oflog = &cout;
179 
180    // bookkeeping
181    setcfg(ResetUnique, 0, "if non-zero, reset the unique number to zero");
182 
183    // use cfg(DT) NOT dt -  dt is part of SatPass...
184    setcfg(DT, -1, "nominal timestep of data (seconds) [required - no default!]");
185    setcfg(Debug, 0, "level of diagnostic output to log, from 0(none) to 7(extreme)");
186    setcfg(useCA1, 0, "use L1 C/A code pseudorange (C1) ()");
187    setcfg(useCA2, 0, "use L2 C/A code pseudorange (C2) ()");
188    setcfg(MaxGap, 180, "maximum allowed time gap within a segment (seconds)");
189    setcfg(MinPts, 13, "minimum number of good points in phase segment ()");
190    setcfg(WLSigma, 1.5, "expected WL sigma (WL cycle) [NB = ~0.83*p-range noise(m)]");
191    setcfg(GFVariation, 16,                    // about 300 5.4-cm wavelengths
192       "expected maximum variation in GF phase in time DT (meters)");
193    // output
194    setcfg(OutputGPSTime, 0,
195       "if 0, output Y,M,D,H,M,S else: W,SoW in edit cmds (log uses SatPass fmt)");
196    setcfg(OutputDeletes, 1,
197       "if non-zero, include delete commands in the output cmd list");
198 
199    // -------------------------------------------------------------------------
200    // advanced options - marked with * - ordinary user will most likely NOT change
201    setcfg(RawBiasLimit, 100, "* change in raw R-Ph that triggers bias reset (m)");
202    // WL editing
203    setcfg(WLNSigmaDelete, 2, "* delete segments with sig(WL) > this * WLSigma ()");
204    setcfg(WLWindowWidth, 50,
205       "* sliding window width for WL slip detection = 10+this/dt) (points)");
206    setcfg(WLNWindows, 2.5,
207       "* minimum segment size for WL small slip search (WLWindowWidth)");
208    setcfg(WLobviousLimit, 3,
209       "* minimum delta(WL) that produces an obvious slip (WLSigma)");
210    setcfg(WLNSigmaStrip, 3.5, "* delete points with WL > this * computed sigma ()");
211    setcfg(WLNptsOutlierStats, 200,
212       "* maximum segment size to use robust outlier detection (pts)");
213    setcfg(WLRobustWeightLimit, 0.35,
214       "* minimum good weight in robust outlier detection (0<wt<=1)");
215    // WL small slips
216    setcfg(WLSlipEdge, 3,
217       "* minimum separating WL slips and end of segment, else edit (pts)");
218    setcfg(WLSlipSize, 0.9, "* minimum WL slip size (WL wavelengths)");
219    setcfg(WLSlipExcess, 0.1,
220       "* minimum amount WL slip must exceed noise (WL wavelengths)");
221    setcfg(WLSlipSeparation, 2.5, "* minimum excess/noise ratio of WL slip ()");
222    // GF small slips
223    setcfg(GFSlipWidth, 5,
224       "* minimum segment length for GF small slip detection (pts)");
225    setcfg(GFSlipEdge, 3,
226       "* minimum separating GF slips and end of segment, else edit (pts)");
227    setcfg(GFobviousLimit, 1,
228       "* minimum delta(GF) that produces an obvious slip (GFVariation)");
229    setcfg(GFSlipOutlier, 5, "* minimum GF outlier magnitude/noise ratio ()");
230    setcfg(GFSlipSize, 0.8, "* minimum GF slip size (5.4cm wavelengths)");
231    setcfg(GFSlipStepToNoise, 0.1, "* maximum GF slip step/noise ratio ()");
232    setcfg(GFSlipToStep, 3, "* minimum GF slip magnitude/step ratio ()");
233    setcfg(GFSlipToNoise, 3, "* minimum GF slip magnitude/noise ratio ()");
234    // GF fix
235    setcfg(GFFixNpts, 15,
236       "* maximum number of points on each side to fix GF slips ()");
237    setcfg(GFFixDegree, 3, "* degree of polynomial used to fix GF slips ()");
238    setcfg(GFFixMaxRMS, 100,
239       "* limit on RMS fit residuals to fix GF slips, else delete (5.4cm)");
240    setcfg(GFSkipSmall, 1,
241       "* if non-zero, skip small GF slips unless there is a WL slip");
242 
243 }
244 catch(Exception& e) { GPSTK_RETHROW(e); }
245 catch(std::exception& e) {
246    Exception E("std except: "+string(e.what())); GPSTK_THROW(E);
247 }
248 catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
249 }
250 
251 //------------------------------------------------------------------------------------
252 //------------------------------------------------------------------------------------
253 // class Segment - used internally only.
254 // An object to hold information about segments = periods of continuous phase.
255 // Keep a linked list of these objects, subdivide whenever a discontinuity is
256 // detected, and join whenever one is fixed.
257 class Segment {
258 public:
259       // member data
260    unsigned int nbeg,nend; // array indexes of the first and last good points
261                            // always maintain these so they point to good data
262    unsigned int npts;      // number of good points in this Segment
263    int nseg;               // segment number - used for data dumps only
264    double bias1;           // bias subtracted from WLbias for WLStats - only
265    Stats<double> WLStats;  // includes N,min,max,ave,sig
266    double bias2;           // bias subtracted from GFP for polynomial fit - only
267    PolyFit<double> PF;     // for fit to GF range
268    double RMSROF;          // RMS residual of fit of polynomial (PF) to GFR
269    bool WLsweep;           // WLstatSweep(this) was called, used by detectWLsmallSlips
270 
271       // member functions
Segment(void)272    Segment(void) : nbeg(0),nend(0),npts(0),nseg(0),bias1(0.0),bias2(0.0)
273       { WLStats.Reset(); WLsweep=false; }
Segment(const Segment & s)274    Segment(const Segment& s)
275       { *this = s; }
~Segment(void)276    ~Segment(void) { }
operator =(const Segment & s)277    Segment& operator=(const Segment& s) {
278       if(this==&s) return *this;
279       nbeg = s.nbeg; nend = s.nend; npts = s.npts; nseg = s.nseg;
280       bias1 = s.bias1; bias2 = s.bias2;
281       WLStats = s.WLStats; PF = s.PF; RMSROF = s.RMSROF; WLsweep = s.WLsweep;
282       return *this;
283    }
284 
285 }; // end class Segment
286 
287 //------------------------------------------------------------------------------------
288 //------------------------------------------------------------------------------------
289 // class Slip - used internally only
290 class Slip {
291 public:
292    int index;           // index in arrays where this slip occurs
293    long NWL,N1;         // slip fixes for WL (N1-N2) and GF (=N1)
294    string msg;          // string to be output after '#' on edit cmds
Slip(int in)295    explicit Slip(int in) : index(in),NWL(0),N1(0) { }
operator <(const Slip & rhs) const296    bool operator<(const Slip &rhs) const { return index < rhs.index; }
297 }; // end class Slip
298 
299 //------------------------------------------------------------------------------------
300 //------------------------------------------------------------------------------------
301 // class GDCPass - inherit SatPass and GDConfiguration - use internally only
302 // this object is used to implement the DC algorithm.
303 class GDCPass : public SatPass, public GDCconfiguration
304 {
305 public:
306    static const unsigned short DETECT;           // both WL and GF
307    static const unsigned short FIX;              // both WL and GF
308    static const unsigned short WLDETECT;
309    static const unsigned short WLFIX;
310    static const unsigned short GFDETECT;
311    static const unsigned short GFFIX;
312 
313    explicit GDCPass(SatPass& sp, const GDCconfiguration& gdc);
314 
315    //~GDCPass(void) { };
316 
317    /// edit obvious outliers, divide into segments using MaxGap
318    /// @throw Exception
319    int preprocess(void);
320 
321    /// compute linear combinations and place the result in the data arrays:
322    /// L1 -> L1;                     L2 -> GFP(m)
323    /// P1 -> WLB(cyc)                P2 -> -GFR(m)
324    /// @throw Exception
325    int linearCombinations(void);
326 
327    /// detect slips in the wide lane bias
328    /// @throw Exception
329    int detectWLslips(void);
330 
331    /// detect obvious slips by computing the first difference (of either WL or GFP)
332    /// and looking for outliers. create new segments where there are slips
333    /// which is either 'WL' or 'GF'.
334    /// @throw Exception
335    int detectObviousSlips(string which);
336 
337    /// compute first differences of data arrays for WL and GF gross slips detection.
338    /// for WL difference the WLbias; for GF, the GFP and the residual GFP-GFR
339    /// Store results in temporary array A1 and A2
340    /// @throw Exception
341    int firstDifferences(string which);
342 
343    /// for one segment, compute conventional statistics on the
344    /// WL bias and count the number of good points
345    /// @throw Exception
346    void WLcomputeStats(list<Segment>::iterator& it);
347 
348    /// for one segment mark bad points that lie outside N*sigma
349    /// delete segments that are too small
350    /// @throw Exception
351    void WLsigmaStrip(list<Segment>::iterator& it);
352 
353    /// for one segment, compute statistics on the WL bias using a
354    /// 'two-paned sliding window', each pane of width 'width' good points.
355    /// store the results in the parallel (to SatPass::data) arrays A1,A2.
356    /// @throw Exception
357    int WLstatSweep(list<Segment>::iterator& it, int width);
358 
359    /// detect slips in all segments using the results of WLstatSweep()
360    /// if close to either end (< window width), just chop off the small segment
361    /// when a slip is found, create a new segment
362    /// also compute conventional stats for each Segment, store in Segment.WLStats
363    /// @throw Exception
364    int detectWLsmallSlips(void);
365 
366    /// determine if a slip has been found at index i, in segment nseg (0..)
367    /// which is associated with it.
368    /// conditions for a slip to be detected:
369    /// 1. test must be >~ 0.67 wlwl
370    /// 2. limit must be much smaller than test
371    /// 3. slip must be far (>1/2 window) from either end
372    /// 4. test must be at a local maximum (~ 2 window widths)
373    /// 5. limit must be at a local minimum (")
374    /// also, large limit (esp near end of a pass) means too much noise, and
375    /// @throw Exception
376    bool foundWLsmallSlip(list<Segment>::iterator& it, int i);
377 
378    /// estimate slips in the WL bias and adjust biases appropriately - ie fix WL slips
379    /// also compute stats for WL for the whole pass
380    /// @throw Exception
381    int fixAllSlips(string which);
382 
383    /// fix the slip at the beginning of the segment pointed to by kt,
384    /// which is the string 'WL' or 'GF'.
385    /// @throw Exception
386    void fixOneSlip(list<Segment>::iterator& kt, string which);
387 
388    /// fix the slip between segments pointed to by left and right
389    /// @throw Exception
390    void WLslipFix(list<Segment>::iterator& left,
391                   list<Segment>::iterator& right);
392 
393    /// fit a polynomial to the GF range, and replace P2 (-gfr) with the residual
394    /// gfp+fit(gfr); divide both P1(gfp) and P2(residual) by wlgf to convert to cycles
395    /// also place the residual gfp+gfr(cycles) in L1
396    /// @throw Exception
397    int prepareGFdata(void);
398 
399    /// detect slips in the geometry-free phase
400    /// @throw Exception
401    int detectGFslips(void);
402 
403    /// for each segment, fit a polynomial to the gfr, then compute and store the
404    /// residual of fit; at the same time, compute stats on the first difference of GF
405    /// @throw Exception
406    int GFphaseResiduals(list<Segment>::iterator& it);
407 
408    /// detect small slips in the geometry-free phase using its first difference
409    /// compute statistics in two windows of fixed width on either side of the point
410    /// of interest and use these to find slips and outliers
411    /// @throw Exception
412    int detectGFsmallSlips(void);
413 
414    /// determine if there is an outlier in the GF phase, using the
415    /// GFP first difference and the statistics computed in detectGFsmallSlips().
416    /// Criteria:
417    /// 1. adjacent first differences have different signs
418    /// 2. they have approximately the same magnitude
419    /// 3. that magnitude is large compared to the noise in the dGFP
420    /// @throw Exception
421    bool foundGFoutlier(int i,int inew,Stats<double>& pastSt,Stats<double>& futureSt);
422 
423    /// determine if a small GF slip is found, using the first differenced gfp
424    /// and statistics computed in detectGFsmallSlips()
425    /// Criteria:
426    /// 1. slip is non-trivial - at least one wavelength
427    /// 2. the change in the average 1stDiff(GFP) across the slip is small
428    /// 3. the slip itself is large compared to the average on either side
429    /// 4. the slip itself is large compared to the noise
430    /// Declare 'slips' that are very near the ends of the segment as outliers
431    /// Conservatively, ignore small slips that are near the level of the noise,
432    /// unless there was a WL slip at the same epoch.
433    /// @throw Exception
434    bool foundGFsmallSlip(int i, int nseg, int iend, int ibeg,
435       deque<int>& pastIn, deque<int>& futureIn,
436                          Stats<double>& pastSt, Stats<double>& futureSt);
437 
438    /// fix the slip between segments pointed to by left and right
439    /// @throw Exception
440    void GFslipFix(list<Segment>::iterator& left,
441                   list<Segment>::iterator& right);
442 
443    /// estimate the size of the slip between segments left and right,
444    /// using points from indexes nb to ne; n1 is the initial estimate of the slip
445    /// called by GFslipFix()
446    /// @throw Exception
447    long EstimateGFslipFix(list<Segment>::iterator& left,
448                           list<Segment>::iterator& right,
449                           unsigned int nb, unsigned int ne, long n1);
450 
451    /// final check on consistency of WL slip fixes with GF slip detection
452    /// @throw Exception
453    int WLconsistencyCheck(void);
454 
455    /// last call before returning: copy edited data back into caller's SatPass,
456    /// generate editing commands, and print and return the final summary.
457    /// @throw Exception
458    string finish(int iret, SatPass& svp, vector<string>& editCmds);
459 
460    /// create a new segment from the given one, starting at index ibeg,
461    /// and insert it after the given iterator.
462    /// Return an iterator pointing to the new segment. String msg is for debug output
463    /// @throw Exception
464    list<Segment>::iterator createSegment(list<Segment>::iterator sit,
465                                          int ibeg,
466                                          string msg=string());
467 
468    /// dump a list of the segments, detail dependent on level
469    /// level=0 one line summary (number of segments)
470    /// level=1 one per line list of segments
471    /// level=2 dump all data, including (if extra) temporary arrays
472    /// return the level 1 output as a string
473    /// @throw Exception
474    string dumpSegments(string msg, int level=2, bool extra=false);
475 
476    /// delete (set all points bad) segment it, optional message
477    /// is used in debug print
478    /// @throw Exception
479    void deleteSegment(list<Segment>::iterator& it, string msg=string());
480 
481 private:
482 
483    /// define this function so that invalid labels will throw, because
484    /// this fails silently #define cfg(a) CFG[#a]     // stringize
485    /// @throw Exception
cfg_func(string a)486    double cfg_func(string a)
487    {
488       if(CFGdescription[a] == string()) {
489          Exception e("cfg(UNKNOWN LABEL) : " + a);
490          GPSTK_THROW(e);
491       }
492       return CFG[a];
493    }
494 
495    /// list of Segments, always in time order, of segments of
496    /// continuous data within the SVPass.
497    list<Segment> SegList;
498 
499    /// list of Slips found; used to generate the editing commands on output
500    list<Slip> SlipList;
501 
502    /// temporary storage arrays, parallel to SatPass::data
503    //vector<double> A1,A2;
504 
505    /// stats on the WL bias after editing for the entire pass
506    Stats<double> WLPassStats;
507 
508    /// stats on the first difference of GF after detectObviousSlips(GF)
509    Stats<double> GFPassStats;
510 
511    /// polynomial fit to the geometry-free range for the whole pass
512    //07202010 not used PolyFit<double> GFPassFit;
513 
514    /// keep count of various results: slips, deletions, etc.; print to log in finish()
515    map<string,int> learn;
516 
517 }; // end class GDCPass
518 
519 //------------------------------------------------------------------------------------
520 // conveniences...
521 #define cfg(a) cfg_func(#a)
522 #define log *(p_oflog)
523 static const int L1 = 0;
524 static const int L2 = 1;
525 static const int P1 = 2;
526 static const int P2 = 3;
527 static const int A1 = 4;
528 static const int A2 = 5;
529 vector<string> DCobstypes; // indexes into both data and this vector are L1,L2,etc...
530 
531 //------------------------------------------------------------------------------------
532 // Return values (used by all routines within this module):
533 static const int GLOfailed=-6;
534 static const int BadInput=-5;
535 static const int NoData=-4;
536 static const int FatalProblem=-3;
537 static const int PrematureEnd=-2;      // NB never used
538 static const int Singular=-1;
539 static const int ReturnOK=0;
540 
541 //------------------------------------------------------------------------------------
542 // these are used only to associate a unique number in the log file with each pass
543 static int GDCUnique=0;     // unique number for each call
544 static int GDCUniqueFix;    // unique for each (WL,GF) fix
545 static string GDCtag="GDC"; // begin each line of return message
546 
547 //------------------------------------------------------------------------------------
548 // wavelength and other frequency-dependent quantities, determined early in DC()
549 // constants used in linear combinations
550 int GLOn;
551 double wl1,wl2,wlwl,wlgf;        // wavelengths: L1,L2,widelane,narrowlane
552 double wl1r,wl2r,wl1p,wl2p;      // coefficients in widelane linear combinations
553 double gf1r,gf2r,gf1p,gf2p;      // coefficients in geometry-free linear combinations
554 
555 //------------------------------------------------------------------------------------
556 // Flags - constants used to mark slips, etc. using the SatPass flag:
557 //------------------------------------------------------------------------------------
558 // These are from SatPass.cpp
559 //const unsigned short SatPass::BAD = 0; // used by caller and DC to mark bad data
560 //const unsigned short SatPass::OK  = 1; // good data, no discontinuity
561 //const unsigned short SatPass::LL1 = 2; // good data, discontinuity on L1 only
562 //const unsigned short SatPass::LL2 = 4; // good data, discontinuity on L2 only
563 //const unsigned short SatPass::LL3 = 6; // good data, discontinuity on L1 and L2
564 const unsigned short GDCPass::WLDETECT =   2;
565 const unsigned short GDCPass::GFDETECT =   4;
566 const unsigned short GDCPass::DETECT   =   6;  // = WLDETECT | GFDETECT
567 const unsigned short GDCPass::WLFIX    =   8;
568 const unsigned short GDCPass::GFFIX    =  16;
569 const unsigned short GDCPass::FIX      =  24;  // = WLFIX | GFFIX
570 
571 // notes on the use of these flags:
572 // if(flag & DETECT) is true for EITHER WL or GF or both
573 // if(flag & FIX)  is true for EITHER WL or GF or both
574 // if((flag & WLDETECT) && (flag & GFDETECT)) is true only for both WL and GF
575 
576 // NB typical slip will have flag = DETECT+OK+FIX = 31
577 //    typical unfixed slip   flag = DETECT+OK     =  7
578 
579 // BAD is used either as flag == BAD (for bad data) or flag != BAD (for good data),
580 // thus there are two gotcha's
581 //   - if a point is marked, but is later set BAD, that info is lost
582 //   - if a BAD point is marked, it becomes 'good'
583 // To avoid this we have to use OK rather than BAD:
584 // either !(flag & OK) or (flag ^ OK) for bad data, and (flag & OK) for good data
585 
586 //------------------------------------------------------------------------------------
587 // The discontinuity corrector function
588 //------------------------------------------------------------------------------------
589 // yes you need the gpstk::
DiscontinuityCorrector(SatPass & svp,GDCconfiguration & gdc,vector<string> & editCmds,string & retMessage,int GLOn_in)590 int gpstk::DiscontinuityCorrector(SatPass& svp,
591                                   GDCconfiguration& gdc,
592                                   vector<string>& editCmds,
593                                   string& retMessage,
594                                   int GLOn_in)
595 {
596 try {
597    unsigned int i,j;
598    int iret;
599 
600    if(gdc.getParameter("ResetUnique") != 0)
601       { GDCUnique=0; gdc.setParameter("ResetUnique=0"); }
602    GDCUnique++;
603 
604    //if(!retMessage.empty()) { GDCtag = retMessage; }
605    retMessage = "";
606 
607    // --------------------------------------------------------------------------------
608    // require obstypes L1,L2,C1/P1,C2/P2, and add two auxiliary arrays
609    DCobstypes.clear();
610    DCobstypes.push_back("L1");
611    DCobstypes.push_back("L2");
612    DCobstypes.push_back((int(gdc.getParameter("useCA1"))) == 0 ? "P1" : "C1");
613    DCobstypes.push_back((int(gdc.getParameter("useCA2"))) == 0 ? "P2" : "C2");
614    DCobstypes.push_back("A1");
615    DCobstypes.push_back("A2");
616 
617    // --------------------------------------------------------------------------------
618    // test input for (a) some data and (b) the required obs types L1,L2,C1/P1,P2
619    vector<double> newdata(6);
620    string found;
621    try {
622       newdata[L1] = svp.data(0,DCobstypes[L1]); found += " " + DCobstypes[L1];
623       newdata[L2] = svp.data(0,DCobstypes[L2]); found += " " + DCobstypes[L2];
624       newdata[P1] = svp.data(0,DCobstypes[P1]); found += " " + DCobstypes[P1];
625       newdata[P2] = svp.data(0,DCobstypes[P2]); found += " " + DCobstypes[P2];
626    }
627    catch (Exception& e) { // if obs type is not found in input
628       ostringstream oss;
629       oss << "   Missing required obs types. Require";
630       for(i=0; i<4; i++) oss << " " << DCobstypes[i];
631       oss << "; found only" << found;
632       retMessage = oss.str();
633       return BadInput;
634    }
635 
636    // --------------------------------------------------------------------------------
637    // create a SatPass using DCobstypes, and fill from input
638    RinexSatID sat(svp.getSat());
639    SatPass nsvp(sat, svp.getDT(), DCobstypes);
640 
641    // fill the new SatPass with the input data
642    nsvp.status() = svp.status();
643    vector<unsigned short> lli(6),ssi(6);
644    for(i=0; i<static_cast<int>(svp.size()); i++) {
645       for(j=0; j<6; j++) {
646          newdata[j] = j < 4 ? svp.data(i,DCobstypes[j]) : 0.0;
647          lli[j] = j < 4 ? svp.LLI(i,DCobstypes[j]) : 0;
648          ssi[j] = j < 4 ? svp.SSI(i,DCobstypes[j]) : 0;
649       }
650       // return value must be 0
651       nsvp.addData(svp.time(i), DCobstypes, newdata, lli, ssi, svp.getFlag(i));
652    }
653 
654    // --------------------------------------------------------------------------------
655    // create a GDCPass from the input SatPass (modified) and GDC configuration
656    GDCPass gp(nsvp,gdc);
657 
658    // --------------------------------------------------------------------------------
659    // if the satellite is Glonass, compute the frequency channel, if necessary,
660    // and define wavelengths and other constants for this satellite
661    GLOn = GLOn_in;
662    if(sat.system == SatelliteSystem::Glonass) {
663 
664       // only compute it if it is out of range
665       if(GLOn < -7 || GLOn > 7) {
666          string msg;
667          // call SatPass::getGLOchannel() to get channel from data
668          GLOn = 0;
669          if(gp.getGLOchannel(GLOn,msg)) {
670             //log << "Computed GLONASS frequency channel = " << GLOn
671             //   << "\n   (" << msg << ")" << endl;
672          }
673          else {
674             ostringstream oss;
675             oss << GDCtag << " " << setw(3) << GDCUnique << " " << sat
676                << " " << printTime(svp.getFirstTime(),svp.outFormat)
677                << " is returning with error code: failed to find GLONASS frequency\n"
678                << msg << endl;
679             retMessage = oss.str();
680             return GLOfailed;
681          }
682       }
683 
684       // GLO Frequency(Hz) L1 is 1602.0e6 + n*562.5e3 Hz = 9 * (178 + n*0.0625) MHz
685       //                   L2    1246.0e6 + n*437.5e3 Hz = 7 * (178 + n*0.0625) MHz
686       // Note that L1/L2 is always 9/7 for freq, 7/9 for wavelength
687       static const double GLOfreq0L1=1602.0e6;
688       static const double GLOdfreqL1= 562.5e3;
689       static const double GLOfreq0L2=1246.0e6;
690       static const double GLOdfreqL2= 437.5e3;
691       static const double F1oF2 = 9.0/7.0;
692       static const double F2oF1 = 7.0/9.0;
693 
694       wl1 = C_MPS/(GLOfreq0L1 + GLOn*GLOdfreqL1);
695       wl2 = C_MPS/(GLOfreq0L2 + GLOn*GLOdfreqL2);
696       wlwl = 1.0 / (1.0/wl1 - 1.0/wl2);
697       wlgf = wl2 - wl1;
698 
699       wl1r = 1.0/(1.0+F2oF1);
700       wl2r = 1.0/(1.0+F1oF2);
701       wl1p = wl1/(1.0-F2oF1);
702       wl2p = wl2/(1.0-F1oF2);
703 
704       gf1r = -1.0;
705       gf2r = 1.0;
706       gf1p = wl1;
707       gf2p = -wl2;
708    }
709    else {                                                   // GPS satellite
710       static const double CFF=C_MPS/OSC_FREQ_GPS;
711       static const double wl1_GPS = CFF/L1_MULT_GPS;            // 19.0cm
712       static const double wl2_GPS = CFF/L2_MULT_GPS;            // 24.4cm
713       static const double wlwl_GPS = CFF/(L1_MULT_GPS-L2_MULT_GPS); // 86.2cm
714       static const double wlgf_GPS = wl2_GPS - wl1_GPS;     //  5.4cm
715       static const double F1oF2 = L1_MULT_GPS/L2_MULT_GPS;          // 77/60
716       static const double F2oF1 = L2_MULT_GPS/L1_MULT_GPS;          // 60/77
717 
718       wl1 = wl1_GPS;
719       wl2 = wl2_GPS;
720       wlwl = wlwl_GPS;
721       wlgf = wlgf_GPS;
722 
723       wl1r = 1.0/(1.0+F2oF1);
724       wl2r = 1.0/(1.0+F1oF2);
725       wl1p = wl1/(1.0-F2oF1);
726       wl2p = wl2/(1.0-F1oF2);
727 
728       gf1r = -1.0;
729       gf2r = 1.0;
730       gf1p = wl1;
731       gf2p = -wl2;
732    }
733 
734    // --------------------------------------------------------------------------------
735    // implement the DC algorithm using the GDCPass
736    // NB search for 'change the arrays' for places where arrays are re-defined
737    // NB search for 'change the data' for places where the data is modified (! biases)
738    // NB search for 'change the bias' for places where the bias is changed
739    for(;;) {      // a convenience...
740       // preparation
741       if( (iret = gp.preprocess() )) break;
742       if( (iret = gp.linearCombinations() )) break;
743 
744       // WL
745       if( (iret = gp.detectWLslips() )) break;
746       if( (iret = gp.fixAllSlips("WL") )) break;
747 
748       // GF
749       if( (iret = gp.prepareGFdata() )) break;
750       if( (iret = gp.detectGFslips() )) break;
751       if( (iret = gp.WLconsistencyCheck() )) break;
752       if( (iret = gp.fixAllSlips("GF") )) break;
753 
754       break;      // mandatory
755    }
756 
757    // --------------------------------------------------------------------------------
758    // generate editing commands for deleted (flagged) data and slips,
759    // use editing command (slips and deletes) to modify the original SatPass data
760    // and print ending summary
761    retMessage = gp.finish(iret, svp, editCmds);
762 
763    return iret;
764 }
765 catch(Exception& e) { GPSTK_RETHROW(e); }
766 catch(std::exception& e) {
767    Exception E("std except: "+string(e.what())); GPSTK_THROW(E);
768 }
769 catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
770 }
771 
772 //------------------------------------------------------------------------------------
773 //------------------------------------------------------------------------------------
774 // class GDCPass member functions
775 //------------------------------------------------------------------------------------
GDCPass(SatPass & sp,const GDCconfiguration & gdc)776 GDCPass::GDCPass(SatPass& sp, const GDCconfiguration& gdc)
777       : SatPass(sp.getSat(), sp.getDT(), sp.getObsTypes())
778 {
779    int i,j;
780    Status = sp.status();
781    dt = sp.getDT();
782    sat = sp.getSat();
783    vector<string> ot = sp.getObsTypes();
784    for(i=0; i<static_cast<int>(ot.size()); i++) {
785       labelForIndex[i] = ot[i];
786       indexForLabel[labelForIndex[i]] = i;
787    }
788 
789    vector<double> vdata;
790    vector<unsigned short> lli,ssi;
791    for(i=0; i<static_cast<int>(sp.size()); i++) {
792       vdata.clear();
793       lli.clear();
794       ssi.clear();
795       for(j=0; j<static_cast<int>(ot.size()); j++) {
796          vdata.push_back(sp.data(i,ot[j]));
797          lli.push_back(sp.LLI(i,ot[j]));
798          ssi.push_back(sp.SSI(i,ot[j]));
799       }
800       addData(sp.time(i),ot,vdata,lli,ssi,sp.getFlag(i));
801    }
802 
803    *((GDCconfiguration*)this) = gdc;
804 
805    learn.clear();
806 }
807 
808 //------------------------------------------------------------------------------------
preprocess(void)809 int GDCPass::preprocess(void)
810 {
811 try {
812    int ilast;
813    unsigned int i,Ngood;
814    double biasL1,biasL2,dbias;
815    list<Segment>::iterator it;
816 
817    if(cfg(Debug) >= 2) {
818       Epoch CurrentTime;
819       //CurrentTime.setLocalTime();
820       log << "\n======== Beg GPSTK Discontinuity Corrector " << GDCUnique
821          << " ================================================\n";
822       log << "GPSTK Discontinuity Corrector Ver. " << GDCVersion << " Run "
823          << CurrentTime << endl;
824    }
825 
826       // check input
827    if(cfg(DT) <= 0) {
828       log << "Error: data time interval is not set...Abort" << endl;
829       return FatalProblem;
830    }
831 
832    // 050109 some parameters should depend on DT
833    CFG["WLWindowWidth"] = 10 + int(0.5+CFG["WLWindowWidth"]/CFG["DT"]);
834 
835       // create the first segment
836    SegList.clear();
837    {
838       Segment s;
839       s.nseg = 1;
840       SegList.push_back(s);
841    }
842    it = SegList.begin();
843 
844       // loop over points in the pass
845       // editing obviously bad data and adding segments where necessary
846    for(ilast=-1,i=0; i<static_cast<int>(size()); i++) {
847 
848       // ignore data the caller has marked BAD
849       if(!(spdvector[i].flag & OK)) continue;
850 
851       // just in case the caller has set it to something else...
852       spdvector[i].flag = OK;
853 
854          // look for obvious outliers
855          // Don't do this - sometimes the pseudoranges get extreme values b/c the
856          // clock is allowed to run off for long times - perfectly normal
857       //if(spdvector[i].data[P1] < cfg(MinRange) ||
858       //   spdvector[i].data[P1] > cfg(MaxRange) ||
859       //   spdvector[i].data[P2] < cfg(MinRange) ||
860       //   spdvector[i].data[P2] > cfg(MaxRange) )
861       //{
862       //   spdvector[i].flag = BAD;
863       //   learn["points deleted: obvious outlier"]++;
864       //   if(cfg(Debug) > 6)
865       //      log << "Obvious outlier " << GDCUnique << " " << sat
866       //         << " at " << i << " " << printTime(time(i),outFormat) << endl;
867       //   continue;
868       //}
869 
870          // note first good point
871       if(ilast == -1) ilast = it->nbeg = i;
872 
873          // is there a gap here? if yes, create a new segment
874          // TD? do this here? why not allow any gap in the WL, and look for gaps
875          // TD? only in the GF?
876       if(cfg(DT)*(i-ilast) > cfg(MaxGap))
877          it = createSegment(it,i,"initial gap");
878 
879          // count good points
880       it->npts++;
881       ilast = i;
882    }
883 
884       // note last good point
885    if(ilast == -1) ilast = it->nbeg;
886    it->nend = ilast;
887 
888       // 'change the arrays' A1, A2 to be range minus phase for output
889       // do the same at the end ("AFT")
890       // loop over segments, counting the number of non-trivial ones
891    for(Ngood=0,it=SegList.begin(); it != SegList.end(); it++) {
892       biasL1 = biasL2 = 0.0;
893 
894          // loop over points in this segment
895       for(i=it->nbeg; i<=it->nend; i++) {
896          if(!(spdvector[i].flag & OK)) continue;
897 
898          dbias = fabs(spdvector[i].data[P1]-wl1*spdvector[i].data[L1]-biasL1);
899          if(dbias > cfg(RawBiasLimit)) {
900             if(cfg(Debug) >= 2) log << "BEFresetL1 " << GDCUnique
901                << " " << sat << " " << printTime(time(i),outFormat)
902                << " " << fixed << setprecision(3) << biasL1
903                << " " << spdvector[i].data[P1] - wl1 * spdvector[i].data[L1] << endl;
904             biasL1 = spdvector[i].data[P1] - wl1 * spdvector[i].data[L1];
905          }
906 
907          dbias = fabs(spdvector[i].data[P2]-wl2*spdvector[i].data[L2]-biasL2);
908          if(dbias > cfg(RawBiasLimit)) {
909             if(cfg(Debug) >= 2) log << "BEFresetL2 " << GDCUnique
910                << " " << sat << " " << printTime(time(i),outFormat)
911                << " " << fixed << setprecision(3) << biasL2
912                << " " << spdvector[i].data[P2] - wl2 * spdvector[i].data[L2] << endl;
913             biasL2 = spdvector[i].data[P2] - wl2 * spdvector[i].data[L2];
914          }
915 
916          spdvector[i].data[A1] =
917             spdvector[i].data[P1] - wl1 * spdvector[i].data[L1] - biasL1;
918          spdvector[i].data[A2] =
919             spdvector[i].data[P2] - wl2 * spdvector[i].data[L2] - biasL2;
920 
921       }  // end loop over points in the segment
922 
923          // delete small segments
924       if(it->npts < static_cast<unsigned int>(cfg(MinPts)))
925          deleteSegment(it,"insufficient data in segment");
926       else
927          Ngood++;
928    }
929 
930    if(cfg(Debug) >= 2) dumpSegments("BEF",2,true);
931 
932    if(Ngood == 0) return NoData;
933    return ReturnOK;
934 }
935 catch(Exception& e) { GPSTK_RETHROW(e); }
936 catch(std::exception& e) {
937    Exception E("std except: "+string(e.what())); GPSTK_THROW(E);
938 }
939 catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
940 }
941 
942 //------------------------------------------------------------------------------------
943 //------------------------------------------------------------------------------------
linearCombinations(void)944 int GDCPass::linearCombinations(void)
945 {
946 try {
947    unsigned int i;
948    double wlr,wlp,wlbias,gfr,gfp;
949    list<Segment>::iterator it;
950 
951    // iterate over segments
952    for(it=SegList.begin(); it != SegList.end(); it++) {
953       it->npts = 0;                       // re-compute npts here
954 
955       // loop over points in this segment
956       for(i=it->nbeg; i<=it->nend; i++) {
957          if(!(spdvector[i].flag & OK)) continue;
958 
959          // narrow lane range (m)
960          wlr = wl1r * spdvector[i].data[P1] + wl2r * spdvector[i].data[P2];
961          // wide lane phase (m)
962          wlp = wl1p * spdvector[i].data[L1] + wl2p * spdvector[i].data[L2];
963          // geometry-free range (m)
964          gfr =        spdvector[i].data[P1] -        spdvector[i].data[P2];
965          // geometry-free phase (m)
966          gfp = gf1p * spdvector[i].data[L1] + gf2p * spdvector[i].data[L2];
967          // wide lane bias (cycles)
968          wlbias = (wlp-wlr)/wlwl;
969 
970          // change the bias
971          if(it->npts == 0) {                             // first good point
972             it->bias1 = wlbias;                          // WL bias (NWL)
973             it->bias2 = gfp;                             // GFP bias
974          }
975 
976          // change the arrays
977          spdvector[i].data[L1] = gfp + gfr;              // only used in GF
978          spdvector[i].data[L2] = gfp;
979          spdvector[i].data[P1] = wlbias;
980          spdvector[i].data[P2] = - gfr;
981 
982          it->npts++;
983       }
984    }
985 
986    if(cfg(Debug) >= 2) dumpSegments("LCD");
987 
988    return ReturnOK;
989 }
990 catch(Exception& e) { GPSTK_RETHROW(e); }
991 catch(std::exception& e) {
992    Exception E("std except: "+string(e.what())); GPSTK_THROW(E);
993 }
994 catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
995 }
996 
997 //------------------------------------------------------------------------------------
998 //------------------------------------------------------------------------------------
999 // detect slips in the wide lane bias
detectWLslips(void)1000 int GDCPass::detectWLslips(void)
1001 {
1002 try {
1003    int iret;
1004    list<Segment>::iterator it;
1005 
1006    // look for obvious slips. this will break one segment into many
1007    if( (iret = detectObviousSlips("WL"))) return iret;
1008 
1009    for(it=SegList.begin(); it != SegList.end(); it++) {
1010 
1011       // compute stats and delete segments that are too small
1012       WLcomputeStats(it);
1013 
1014       // sigma-strip the WL bias, and remove small segments
1015       if(it->npts > 0) WLsigmaStrip(it);
1016 
1017       // print this before deleting segments with large sigma
1018       if(cfg(Debug) >= 1 && it->npts >= static_cast<unsigned int>(cfg(MinPts)))
1019          log << "WLSIG " << GDCUnique << " " << sat
1020             << " " << it->nseg
1021             << " " << printTime(time(it->nbeg),outFormat)
1022             << fixed << setprecision(3)
1023             << " " << it->WLStats.StdDev()
1024             << " " << it->WLStats.Average()
1025             << " " << it->WLStats.Minimum()
1026             << " " << it->WLStats.Maximum()
1027             << " " << it->npts
1028             << " " << it->nbeg << " - " << it->nend
1029             << " " << it->bias1
1030             << " " << it->bias2
1031             << endl;
1032 
1033       // delete segments if sigma is too high...
1034       if(it->WLStats.StdDev() > cfg(WLNSigmaDelete)*cfg(WLSigma))
1035          deleteSegment(it,"WL sigma too big");
1036 
1037       // if there are less than about 2.5*cfg(WLWindowWidth) good points, don't bother
1038       // to use the sliding window method to look for slips; otherwise
1039       // compute stats for each segment using the 'two-paned sliding stats window',
1040       // store results in the temporary arrays
1041       if(double(it->npts) >= cfg(WLNWindows) * int(cfg(WLWindowWidth))) {
1042          iret = WLstatSweep(it,int(cfg(WLWindowWidth)));
1043          if(iret) return iret;
1044       }
1045 
1046    }  // end loop over segments
1047 
1048    // use the temporary arrays filled by WLstatSweep to detect slips in the WL bias
1049    // recompute WLstats, and break up the segments where slips are found
1050    if( (iret = detectWLsmallSlips())) return iret;
1051 
1052    // delete all segments that are too small
1053    for(it=SegList.begin(); it != SegList.end(); it++) {
1054       if(it->npts < static_cast<unsigned int>(cfg(MinPts)))
1055          deleteSegment(it,"insufficient data in segment");
1056    }
1057 
1058    if(cfg(Debug) >= 4) dumpSegments("WLD");
1059 
1060    return ReturnOK;
1061 }
1062 catch(Exception& e) { GPSTK_RETHROW(e); }
1063 catch(std::exception& e) {
1064    Exception E("std except: "+string(e.what())); GPSTK_THROW(E);
1065 }
1066 catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
1067 }
1068 
1069 //------------------------------------------------------------------------------------
1070 // detect obvious slips by computing the first difference (of either WL or GFP)
1071 // and looking for outliers. create new segments where there are slips
1072 // which is either 'WL' or 'GF'.
detectObviousSlips(string which)1073 int GDCPass::detectObviousSlips(string which)
1074 {
1075 try {
1076    // TD determine from range noise // ~ 2*range noise/wl2
1077    const double WLobviousNWLLimit=cfg(WLobviousLimit)*cfg(WLSigma);
1078    const double GFobviousNWLLimit=cfg(GFobviousLimit)*cfg(GFVariation)/wlgf;
1079    bool outlier,nogood;
1080    unsigned int i,j,ibad,igood,nok;
1081    int iret;
1082    double limit,wlbias;
1083    list<Segment>::iterator it;
1084 
1085    // compute 1st differences of (WL bias, GFP-GFR) as 'which' is (WL,GF)
1086    iret = firstDifferences(which);
1087    if(iret) return iret;
1088 
1089    if(cfg(Debug) >= 5) dumpSegments(string("D")+which,2,true); // DWL DGF
1090 
1091    // scan the first differences, eliminate outliers and
1092    // break into segments where there are WL slips.
1093    limit = (which == string("WL") ? WLobviousNWLLimit : GFobviousNWLLimit);
1094    it = SegList.begin();
1095    nok = 0;
1096    nogood = true;
1097    outlier = false;
1098    for(i=0; i<static_cast<int>(size()); i++) {
1099       if(i < it->nbeg) {
1100          outlier = false;
1101          continue;
1102       }
1103       if(i > it->nend) {                  // change segments
1104          if(outlier) {
1105             if(spdvector[ibad].flag & OK) nok--;
1106             spdvector[ibad].flag = BAD;
1107             learn[string("points deleted: ") + which + string(" slip outlier")]++;
1108             outlier = false;
1109          }
1110          it->npts = nok;
1111          // update nbeg and nend
1112          while(it->nbeg < it->nend
1113             && it->nbeg < static_cast<int>(size())
1114             && !(spdvector[it->nbeg].flag & OK) ) it->nbeg++;
1115          while(it->nend > it->nbeg
1116             && it->nend > 0
1117             && !(spdvector[it->nend].flag & OK) ) it->nend--;
1118          it++;
1119          if(it == SegList.end())
1120             return ReturnOK;
1121          nok = 0;
1122       }
1123 
1124       if(!(spdvector[i].flag & OK))
1125          continue;
1126       nok++;                                   // nok = # good points in segment
1127 
1128       if(nogood) { igood = i; nogood=false; }  // igood is index of last good point
1129 
1130       if(fabs(spdvector[i].data[A1]) > limit) {// found an outlier (1st diff, cycles)
1131          outlier = true;
1132          ibad = i;                             // ibad is index of last bad point
1133       }
1134       else if(outlier) {                       // this point good, but not past one(s)
1135          for(unsigned int j=igood+1; j<ibad; j++) {
1136             if(spdvector[j].flag & OK)
1137                nok--;
1138             if(spdvector[j].flag & DETECT)
1139                log << "Warning - found an obvious slip, "
1140                   << "but marking BAD a point already marked with slip "
1141                   << GDCUnique << " " << sat
1142                   << " " << printTime(time(j),outFormat) << " " << j << endl;
1143             spdvector[j].flag = BAD;             // mark all points between as bad
1144             learn[string("points deleted: ") + which + string(" slip outlier")]++;
1145          }
1146 
1147             // create a new segment, starting at the last outlier
1148          it->npts = nok-2;
1149          // WL slip gross  OR  GF slip gross
1150          it = createSegment(it,ibad,which+string(" slip gross"));
1151 
1152             // mark it
1153          spdvector[ibad].flag |= (which == string("WL") ? WLDETECT : GFDETECT);
1154 
1155             // change the bias in the new segment
1156          if(which == "WL") {
1157             wlbias = spdvector[ibad].data[P1];
1158             it->bias1 = long(wlbias+(wlbias > 0 ? 0.5 : -0.5));   // WL bias (NWL)
1159          }
1160          if(which == "GF")
1161             it->bias2 = spdvector[ibad].data[L2];                 // GFP bias
1162 
1163             // prep for next point
1164          nok = 2;
1165          outlier = false;
1166          igood = ibad;
1167       }
1168       else
1169          igood = i;
1170    }
1171    it->npts = nok;
1172 
1173    return ReturnOK;
1174 }
1175 catch(Exception& e) { GPSTK_RETHROW(e); }
1176 catch(std::exception& e) {
1177    Exception E("std except: "+string(e.what())); GPSTK_THROW(E);
1178 }
1179 catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
1180 }
1181 
1182 //------------------------------------------------------------------------------------
1183 // compute first differences of data array(s) for WL and GF gross slip detection.
1184 // for WL difference the WLbias (P1); for GF, the GFP (L2) and the GFR (P2)
1185 // Store results in A1, and for GF put the range difference in A2
firstDifferences(string which)1186 int GDCPass::firstDifferences(string which)
1187 {
1188 try {
1189    //if(A1.size() != size()) return FatalProblem;
1190 
1191    int i,iprev=-1;
1192 
1193    for(i=0; i<static_cast<int>(size()); i++) {
1194       // ignore bad data
1195       if(!(spdvector[i].flag & OK)) {
1196          spdvector[i].data[A1] = spdvector[i].data[A2] = 0.0;
1197          continue;
1198       }
1199 
1200       // compute first differences - 'change the arrays' A1 and A2
1201       if(which == string("WL")) {
1202          if(iprev == -1)
1203             spdvector[i].data[A1] = 0.0;
1204          else
1205             spdvector[i].data[A1] =
1206                (spdvector[i].data[P1] - spdvector[iprev].data[P1]);
1207       }
1208       else if(which == string("GF")) {
1209          if(iprev == -1)            // first difference not defined at first point
1210             spdvector[i].data[A1] = spdvector[i].data[A2] = 0.0;
1211          else {
1212             // compute first difference of L1 = raw residual GFP-GFR
1213             spdvector[i].data[A1] =
1214                (spdvector[i].data[L1] - spdvector[iprev].data[L1]);
1215             // compute first difference of L2 = GFP
1216             spdvector[i].data[A2] =
1217                (spdvector[i].data[L2] - spdvector[iprev].data[L2]);
1218          }
1219       }
1220 
1221       // go to next point
1222       iprev = i;
1223    }
1224 
1225    return ReturnOK;
1226 }
1227 catch(Exception& e) { GPSTK_RETHROW(e); }
1228 catch(std::exception& e) {
1229    Exception E("std except: "+string(e.what())); GPSTK_THROW(E);
1230 }
1231 catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
1232 }
1233 
1234 //------------------------------------------------------------------------------------
1235 // for one segment, compute conventional statistics on the
1236 // WL bias and count the number of good points
WLcomputeStats(list<Segment>::iterator & it)1237 void GDCPass::WLcomputeStats(list<Segment>::iterator& it)
1238 {
1239 try {
1240    // compute WLStats
1241    it->WLStats.Reset();
1242    it->npts = 0;
1243 
1244    // loop over data, adding to Stats, and counting good points
1245    for(unsigned int i=it->nbeg; i<=it->nend; i++) {
1246       if(!(spdvector[i].flag & OK)) continue;
1247       it->WLStats.Add(spdvector[i].data[P1] - it->bias1);
1248       it->npts++;
1249    }
1250 
1251    // eliminate segments with too few points
1252    if(it->npts < static_cast<unsigned int>(cfg(MinPts)))
1253       deleteSegment(it,"insufficient data in segment");
1254 }
1255 catch(Exception& e) { GPSTK_RETHROW(e); }
1256 catch(std::exception& e) {
1257    Exception E("std except: "+string(e.what())); GPSTK_THROW(E);
1258 }
1259 catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
1260 }
1261 
1262 //------------------------------------------------------------------------------------
1263 // for one segment, compute conventional statistics on the
1264 // WL bias, remove small segments, and mark bad points that lie outside N*sigma
WLsigmaStrip(list<Segment>::iterator & it)1265 void GDCPass::WLsigmaStrip(list<Segment>::iterator& it)
1266 {
1267 try {
1268    bool outlier,haveslip;
1269    unsigned short slip;
1270    int slipindex;
1271    unsigned int i,j,k;
1272    double wlbias,nsigma,ave;
1273 
1274    // use robust stats on small segments, for big ones stick with conventional
1275 
1276    if(it->npts < cfg(WLNptsOutlierStats)) {   // robust
1277       // 'change the arrays' A1 and A2; they will be overwritten by WLstatSweep
1278       // but then dumped as DSCWLF...
1279       // use temp vectors so they can be passed to Robust::MAD and Robust::MEstimate
1280       double median,mad;
1281       vector<double> vecA1,vecA2;      // use these temp, for Robust:: calls
1282 
1283       // put wlbias in vecA1, but without gaps: let j index good points only from nbeg
1284       for(j=i=it->nbeg; i<=it->nend; i++) {
1285          if(!(spdvector[i].flag & OK)) continue;
1286          wlbias = spdvector[i].data[P1] - it->bias1;
1287          vecA1.push_back(wlbias);
1288          vecA2.push_back(0.0);
1289          j++;
1290       }
1291 
1292       mad = Robust::MAD(&(vecA1[0]),j-it->nbeg,median,true);
1293       nsigma = cfg(WLNSigmaStrip) * mad;
1294       ave = Robust::MEstimate(&(vecA1[0]),j-it->nbeg,median,mad,&(vecA2[0]));
1295 
1296       // change the array : A1 is wlbias, A2 (output) will contain the weights
1297       // copy temps out into A1 and A2
1298       for(k=0,i=it->nbeg; i<j; k++,i++) {
1299          spdvector[i].data[A1] = vecA1[k];
1300          spdvector[i].data[A2] = vecA2[k];
1301       }
1302 
1303       haveslip = false;
1304       for(j=i=it->nbeg; i<=it->nend; i++) {
1305          if(!(spdvector[i].flag & OK)) continue;
1306 
1307          wlbias = spdvector[i].data[P1] - it->bias1;
1308 
1309          if(fabs(wlbias-ave) > nsigma ||
1310                spdvector[j].data[A2] < cfg(WLRobustWeightLimit))
1311             outlier = true;
1312          else
1313             outlier = false;
1314 
1315          // remove points by sigma stripping
1316          if(outlier) {
1317             if(spdvector[i].flag & DETECT || i == it->nbeg) {
1318                haveslip = true;
1319                slipindex = i;        // mark
1320                slip = spdvector[i].flag; // save to put on first good point
1321             }
1322             spdvector[i].flag = BAD;
1323             learn["points deleted: WL sigma stripping"]++;
1324             it->npts--;
1325             it->WLStats.Subtract(wlbias);
1326          }
1327          else if(haveslip) {
1328             spdvector[i].flag = slip;
1329             haveslip = false;
1330          }
1331 
1332          if(cfg(Debug) >= 6) {
1333             log << "DSCWLR " << GDCUnique << " " << sat
1334             << " " << it->nseg
1335             << " " << printTime(time(i),outFormat)
1336             << fixed << setprecision(3)
1337             << " " << setw(3) << spdvector[i].flag
1338             << " " << setw(13) << spdvector[j].data[A1] // wlbias
1339             << " " << setw(13) << fabs(wlbias-ave)
1340             << " " << setw(5) << spdvector[j].data[A2]  // 0 <= weight <= 1
1341             << " " << setw(3) << i
1342             << (outlier ? " outlier" : "");
1343             if(i == it->nbeg) log
1344                << " " << setw(13) << it->bias1
1345                << " " << setw(13) << it->bias2;
1346             log << endl;
1347          }
1348 
1349          j++;
1350       }
1351 
1352    }
1353    else {                                             // conventional
1354 
1355       //nsigma = WLsigmaStrippingNsigmaLimit * it->WLStats.StdDev();
1356       nsigma = cfg(WLNSigmaStrip) * it->WLStats.StdDev();
1357 
1358       haveslip = false;
1359       ave = it->WLStats.Average();
1360       for(i=it->nbeg; i<=it->nend; i++) {
1361          if(!(spdvector[i].flag & OK)) continue;
1362 
1363          wlbias = spdvector[i].data[P1] - it->bias1;
1364 
1365          // remove points by sigma stripping
1366          if(fabs(wlbias-ave) > nsigma) { // TD add absolute limit?
1367             if(spdvector[i].flag & DETECT) {
1368                haveslip = true;
1369                slipindex = i;        // mark
1370                slip = spdvector[i].flag; // save to put on first good point
1371             }
1372             spdvector[i].flag = BAD;
1373             learn["points deleted: WL sigma stripping"]++;
1374             it->npts--;
1375             it->WLStats.Subtract(wlbias);
1376          }
1377          else if(haveslip) {
1378             spdvector[i].flag = slip;
1379             haveslip = false;
1380          }
1381 
1382       }  // loop over points in segment
1383    }
1384 
1385    // change nbeg, but don't change the bias
1386    if(haveslip) {
1387       it->nbeg = slipindex;
1388    }
1389 
1390    // again
1391    if(it->npts < static_cast<unsigned int>(cfg(MinPts)))
1392       deleteSegment(it,"WL sigma stripping");
1393    else {
1394       // update nbeg and nend // TD add limit 0 size()
1395       while(it->nbeg < it->nend && !(spdvector[it->nbeg].flag & OK)) it->nbeg++;
1396       while(it->nend > it->nbeg && !(spdvector[it->nend].flag & OK)) it->nend--;
1397    }
1398 
1399 }
1400 catch(Exception& e) { GPSTK_RETHROW(e); }
1401 catch(std::exception& e) {
1402    Exception E("std except: "+string(e.what())); GPSTK_THROW(E);
1403 }
1404 catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
1405 }
1406 
1407 //------------------------------------------------------------------------------------
1408 // in the given segment, compute statistics on the WL bias using a
1409 // 'two-paned sliding window', each pane of width 'width' good points.
1410 // store the results in the parallel (to SatPass::data) arrays A1,A2.
WLstatSweep(list<Segment>::iterator & it,int width)1411 int GDCPass::WLstatSweep(list<Segment>::iterator& it, int width)
1412 {
1413 try {
1414    unsigned int iminus,i,iplus,uwidth(width);
1415    double wlbias,test,limit;
1416    Stats<double> pastStats, futureStats;
1417 
1418    // ignore empty segments
1419    if(it->npts == 0) return ReturnOK;
1420    it->WLsweep = true;
1421 
1422       // Cartoon of the 'two-pane moving window'
1423       // windows:  'past window'      'future window'
1424       // stats  :  --- pastStats----  ----futureStats--
1425       // data   : (x x x x x x x x x)(x x x x x x x x x) x ...
1426       //           |               |  |                  |
1427       // indexes: iminus          i-1 i                 iplus
1428 
1429    // start with the window 'squashed' to one point - the first one
1430    iminus = i = iplus = it->nbeg;
1431 
1432    // fill up the future window to size 'width', but don't go beyond the segment
1433    while(futureStats.N() < uwidth && iplus <= it->nend) {
1434       if(spdvector[iplus].flag & OK) {                // add only good data
1435          futureStats.Add(spdvector[iplus].data[P1] - it->bias1);
1436       }
1437       iplus++;
1438    }
1439 
1440    // now loop over all points in the segment
1441    for(i=it->nbeg; i<= it->nend; i++) {
1442       if(!(spdvector[i].flag & OK))                      // add only good data
1443          continue;
1444 
1445       // compute test and limit
1446       test = 0;
1447       if(pastStats.N() > 0 && futureStats.N() > 0)
1448          test = fabs(futureStats.Average()-pastStats.Average());
1449       limit = ::sqrt(futureStats.Variance() + pastStats.Variance());
1450       // 'change the arrays' A1 and A2
1451       spdvector[i].data[A1] = test;
1452       spdvector[i].data[A2] = limit;
1453 
1454       wlbias = spdvector[i].data[P1] - it->bias1;        // debiased WLbias
1455 
1456       // dump the stats
1457       if(cfg(Debug) >= 6) log << "WLS " << GDCUnique
1458          << " " << sat << " " << it->nseg
1459          << " " << printTime(time(i),outFormat)
1460          << fixed << setprecision(3)
1461          << " " << setw(3) << pastStats.N()
1462          << " " << setw(7) << pastStats.Average()
1463          << " " << setw(7) << pastStats.StdDev()
1464          << " " << setw(3) << futureStats.N()
1465          << " " << setw(7) << futureStats.Average()
1466          << " " << setw(7) << futureStats.StdDev()
1467          << " " << setw(9) << spdvector[i].data[A1]
1468          << " " << setw(9) << spdvector[i].data[A2]
1469          << " " << setw(9) << wlbias
1470          << " " << setw(3) << i
1471          << endl;
1472 
1473       // update stats :
1474       // move point i from future to past, ...
1475       futureStats.Subtract(wlbias);
1476       pastStats.Add(wlbias);
1477       // ... and move iplus up by one (good) point, ...
1478       while(futureStats.N() < uwidth && iplus <= it->nend) {
1479          if(spdvector[iplus].flag & OK) {
1480             futureStats.Add(spdvector[iplus].data[P1] - it->bias1);
1481          }
1482          iplus++;
1483       }
1484       // ... and move iminus up by one good point
1485       while(static_cast<int>(pastStats.N()) > uwidth && iminus <= it->nend) {
1486          if(spdvector[iminus].flag & OK) {
1487             pastStats.Subtract(spdvector[iminus].data[P1] - it->bias1);
1488          }
1489          iminus++;
1490       }
1491 
1492    }  // end loop over i=all points in segment
1493 
1494    return  ReturnOK;
1495 }
1496 catch(Exception& e) { GPSTK_RETHROW(e); }
1497 catch(std::exception& e) {
1498    Exception E("std except: "+string(e.what())); GPSTK_THROW(E);
1499 }
1500 catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
1501 }
1502 
1503 //------------------------------------------------------------------------------------
1504 // look for slips in the WL using the results of WLstatSweep
1505 // if slip is close to either end (< window width), just chop off the small segment
1506 // recompute WLstats; when a slip is found, create a new segment
detectWLsmallSlips(void)1507 int GDCPass::detectWLsmallSlips(void)
1508 {
1509 try {
1510    unsigned int k,nok;
1511    double wlbias;
1512    list<Segment>::iterator it;
1513 
1514    // find first segment for which WLstatSweep was called
1515    it = SegList.begin();
1516    while(!it->WLsweep) {
1517       it++;
1518       if(it == SegList.end()) return ReturnOK;
1519    }
1520    it->WLStats.Reset();
1521 
1522    // loop over the data arrays - all segments
1523    unsigned int i = it->nbeg;
1524    nok = 0;
1525    unsigned int halfwidth = static_cast<unsigned int>(cfg(WLSlipEdge));
1526    while(i < size())
1527    {
1528       // must skip segments for which WLstatSweep was not called
1529       while(i > it->nend || !it->WLsweep) {
1530          if(i > it->nend) {
1531             it->npts = nok;
1532             nok = 0;
1533          }
1534          it++;
1535          if(it == SegList.end()) return ReturnOK;
1536          i = it->nbeg;
1537          if(it->WLsweep) {
1538             it->WLStats.Reset();
1539          }
1540       }
1541 
1542       if(spdvector[i].flag & OK) {
1543          nok++;                                 // nok = # good points in segment
1544 
1545          if(nok == 1) {                         // change the bias, as WLStats reset
1546             wlbias = spdvector[i].data[P1];
1547             it->bias1 = long(wlbias+(wlbias > 0 ? 0.5 : -0.5));
1548          }
1549 
1550          //  condition 3 - near ends of segment?
1551          if(nok < halfwidth || (it->npts - nok) < halfwidth ) {
1552             // failed test 3 - near ends of segment
1553             // consider chopping off this end of segment - large limit?
1554             // TD must do something here ...
1555             if(cfg(Debug) >= 6) log << "too near end " << GDCUnique
1556                << " " << i << " " << nok << " " << it->npts-nok
1557                << " " << printTime(time(i),outFormat)
1558                << " " << spdvector[i].data[A1] << " " << spdvector[i].data[A2]
1559                << endl;
1560          }
1561          else if(foundWLsmallSlip(it,i)) { // met condition 3
1562             // create new segment
1563             // TD what if nok < MinPts? -- cf detectGFsmallSlips
1564             k = it->npts;
1565             it->npts = nok;
1566             //log << "create new segment at i = " << i << " " << nok << "pts\n";
1567             it = createSegment(it,i,"WL slip small");
1568 
1569             // mark it
1570             spdvector[i].flag |= WLDETECT;
1571 
1572             // prep for next segment
1573             // biases remain the same in the new segment
1574             it->npts = k - nok;
1575             nok = 0;
1576             it->WLStats.Reset();
1577             wlbias = spdvector[i].data[P1]; // change the bias, as WLStats reset
1578             it->bias1 = long(wlbias+(wlbias > 0 ? 0.5 : -0.5));
1579          }
1580 
1581          it->WLStats.Add(spdvector[i].data[P1] - it->bias1);
1582 
1583       } // end if good data
1584 
1585       i++;
1586 
1587    }  // end loop over points in the pass
1588    it->npts = nok;
1589 
1590    return ReturnOK;
1591 }
1592 catch(Exception& e) { GPSTK_RETHROW(e); }
1593 catch(std::exception& e) {
1594    Exception E("std except: "+string(e.what())); GPSTK_THROW(E);
1595 }
1596 catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
1597 }
1598 
1599 //------------------------------------------------------------------------------------
1600 // determine if a slip has been found at index i, in segment it
1601 // A1 = test = fabs(futureStats.Average()-pastStats.Average()) ~ step in ave WL
1602 // A2 = limit = sqrt(futureStats.Variance()+pastStats.Variance()) ~ noise in WL
1603 // ALL CONDITIONs needed for a slip to be detected:
1604 // 1. test must be > WLSlipSize (cycles)
1605 // 2. test-limit must be > (WLSlipExcess)       // TD keep this?
1606 // 3. slip must be far (>1/2 window) from either end - handled in detectWLsmallSlips
1607 // 4. test must be at a local maximum within ~ window width
1608 // 5. limit must be at a local minimum within ~ window width
1609 // 6. (test-limit)/limit > (WLSlipSeparation = 2.5)         // this is critical test
1610 // large limit (esp near end of a pass) means too much noise
foundWLsmallSlip(list<Segment>::iterator & it,int i)1611 bool GDCPass::foundWLsmallSlip(list<Segment>::iterator& it, int i)
1612 {
1613 try {
1614    const unsigned int minMaxWidth=int(cfg(WLSlipEdge));
1615    unsigned int j,jp,jm,pass4,pass5,Pass;
1616    // A1 = step = fabs(futureStats.Average() - pastStats.Average());
1617    // A2 = limit = ::sqrt(futureStats.Variance() + pastStats.Variance());
1618    // all units WL cycles
1619    double step = spdvector[i].data[A1];
1620    double lim = spdvector[i].data[A2];
1621 
1622    // 050109 if Debug=6, print only possible slips, if 7 print all
1623    bool isSlip=false, halfCycle=false;
1624    ostringstream oss;
1625 
1626    // Debug print - NB '>' is always pass, '<=' is fail....
1627    if(cfg(Debug) >= 6) oss << "WLslip " << GDCUnique
1628       << " " << sat << " " << setw(2) << it->nseg
1629       << " " << setw(3) << i
1630       << " " << printTime(time(i),outFormat)
1631       //<< " " << it->npts << "pt"
1632       << fixed << setprecision(2)
1633       << " step=" << step << " lim=" << lim
1634       << " (1)" << spdvector[i].data[A1]
1635       << (spdvector[i].data[A1] > cfg(WLSlipSize) ? ">" : "<=")
1636       << cfg(WLSlipSize)
1637       << " (2)" << spdvector[i].data[A1]-spdvector[i].data[A2]
1638       << (spdvector[i].data[A1]-spdvector[i].data[A2]>cfg(WLSlipExcess)?">":"<=")
1639       << cfg(WLSlipExcess); // no endl
1640 
1641    Pass = 0;         // 111312 count all tests passed
1642 
1643    // CONDITION 1
1644    if(step > cfg(WLSlipSize)) Pass++;
1645    else if(step > 0.45) halfCycle = true;
1646    // CONDITION 2 should be step-lim <= fraction of lim ~~ disc > frac * sigma (6!)
1647    if(step-lim > cfg(WLSlipExcess)) Pass++;
1648 
1649    // CONDITION 6 - 111312 put 6 here, its more important
1650    double ratio=(step-lim)/lim;
1651    if(cfg(Debug) >= 6) oss << " (6)" << ratio
1652       << (ratio > cfg(WLSlipSeparation) ? ">" : "<=") << cfg(WLSlipSeparation);
1653    if(ratio > cfg(WLSlipSeparation) ) Pass++;
1654 
1655    // CONDITIONs 4 and 5
1656    //         x
1657    //        x x
1658    //       x   x
1659    //      x     x
1660    //     x       x
1661    //    x         x
1662    // ------------------------
1663    //      jp=012345     jp==0 is i, the current point
1664    // do for minMaxWidth pts on each side of point; best score is pass=2*minMaxWidth
1665    // why is minMaxWidth used here?
1666    double slope=(step-lim)/(8.0*minMaxWidth);
1667    j = pass4 = pass5 = 0;
1668    jp = jm = i;
1669    do {
1670       // find next good point in future
1671       do { jp++; } while(jp < it->nend && !(spdvector[jp].flag & OK));
1672       if(jp >= it->nend) break;
1673          // CONDITION 4: test(A1) is a local maximum
1674       if(spdvector[i].data[A1]-spdvector[jp].data[A1] > j*slope) pass4++;
1675          // CONDITION 5: limit(A2) is a local minimum
1676       if(spdvector[i].data[A2]-spdvector[jp].data[A2] < -(j*slope)) pass5++;
1677 
1678       // find next good point in past
1679       do { jm--; } while(jm > it->nbeg && !(spdvector[jm].flag & OK));
1680       if(jm <= it->nbeg) break;
1681          // CONDITION 4: test(A1) is a local maximum
1682       if(spdvector[i].data[A1]-spdvector[jm].data[A1] > j*slope) pass4++;
1683          // CONDITION 5: limit(A2) is a local minimum
1684       if(spdvector[i].data[A2]-spdvector[jm].data[A2] < -(j*slope)) pass5++;
1685 
1686    } while(++j < minMaxWidth);
1687 
1688    // perfect = 2*minMaxWidth; allow 1 miss..
1689    if(pass4 >= 2*minMaxWidth-1) Pass++;
1690    if(cfg(Debug) >= 6) oss << " (4)" << pass4
1691       << (pass4 >= 2*minMaxWidth-1 ? ">" : "<=") << 2*minMaxWidth-2;
1692    if(pass5 >= 2*minMaxWidth-1) Pass++;
1693    if(cfg(Debug) >= 6) oss << " (5)" << pass5
1694       << (pass5 >= 2*minMaxWidth-1 ? ">" : "<=") << 2*minMaxWidth-2;
1695 
1696    if(Pass == 5) {
1697       if(cfg(Debug) >= 6) oss << " possible WL slip";
1698       isSlip = true;
1699    }
1700 
1701    // half-cycles - warning only - TD detect in GF, and fix
1702    j = 1;
1703    if(!halfCycle) {                          // look for half-cycle-slip > 1
1704       j = 2*step - int(2*step+(step > 0.0 ? 0.5 : -0.5));
1705       slope = ::fabs(2*step-int(2*step));    // slope is a dummy here
1706       if(j%2 || slope < 3*lim) halfCycle=true;
1707    }
1708    if(Pass >= 4 && halfCycle && j != 0) log << "WLslip " << GDCUnique
1709       << " " << sat << " " << setw(2) << it->nseg << " " << setw(3) << i
1710       << " " << printTime(time(i),outFormat)
1711       << " Warning - possible half-cycle slip of " << j << " WL half-cycles\n";
1712 
1713    if(cfg(Debug) >= 6) log << oss.str() << endl;
1714    return isSlip;
1715 }
1716 catch(Exception& e) { GPSTK_RETHROW(e); }
1717 catch(std::exception& e) {
1718    Exception E("std except: "+string(e.what())); GPSTK_THROW(E);
1719 }
1720 catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
1721 }
1722 
1723 //------------------------------------------------------------------------------------
1724 //------------------------------------------------------------------------------------
1725 // estimate slips and adjust biases appropriately - ie fix slips - for both WL and GF
1726 // merge all data into one segment
fixAllSlips(string which)1727 int GDCPass::fixAllSlips(string which)
1728 {
1729 try {
1730    // find the largest segment and start there, always combine the largest with its
1731    // largest neighbor
1732    unsigned int i,nmax;
1733    list<Segment>::iterator it, kt;
1734 
1735    // loop over all segments, erasing empty ones
1736    it = SegList.begin();
1737    while(it != SegList.end()) {
1738       if(it->npts <= 0)
1739          it = SegList.erase(it);
1740       else
1741          it++;
1742    }
1743 
1744    if(SegList.empty())
1745       return NoData;
1746 
1747    // find the largest segment
1748    for(kt=SegList.end(),nmax=0,it=SegList.begin(); it != SegList.end(); it++) {
1749       if(it->npts > nmax) {
1750          nmax = it->npts;
1751          kt = it;
1752       }
1753    }
1754 
1755    // fix all the slips, starting with the largest segment
1756    // this will merge all segments into one
1757    GDCUniqueFix = 0;
1758    while(kt != SegList.end()) {
1759       fixOneSlip(kt,which);
1760    }
1761 
1762    // TD here to return should be a separate call...
1763 
1764    // now compute stats for the WL for the (single segment) whole pass
1765    kt = SegList.begin();
1766    if(which == string("WL")) {                                    // WL
1767       WLPassStats.Reset();
1768       for(i=kt->nbeg; i <= kt->nend; i++) {
1769          if(!(spdvector[i].flag & OK)) continue;
1770          WLPassStats.Add(spdvector[i].data[P1] - kt->bias1);
1771       }
1772    }
1773    // change the biases - reset the GFP bias so that it matches the GFR
1774    else {                                                         // GF
1775       //dumpSegments("GFFbefRebias",2,true); //temp
1776       bool first(true);
1777       for(i=kt->nbeg; i <= kt->nend; i++) {
1778          if(!(spdvector[i].flag & OK)) continue;
1779          if(first) {
1780             first = false;
1781             kt->bias2 = spdvector[i].data[L2] + spdvector[i].data[P2];
1782             kt->bias1 = spdvector[i].data[P1];
1783          }
1784          // change the data - recompute GFR-GFP so it has one consistent bias
1785          spdvector[i].data[L1] = spdvector[i].data[L2] + spdvector[i].data[P2];
1786       }
1787    }
1788 
1789    if(cfg(Debug) >= 3) dumpSegments(which + string("F"),2,true);   // DSCWLF DSCGFF
1790 
1791    return ReturnOK;
1792 }
1793 catch(Exception& e) { GPSTK_RETHROW(e); }
1794 catch(std::exception& e) {
1795    Exception E("std except: "+string(e.what())); GPSTK_THROW(E);
1796 }
1797 catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
1798 }
1799 
1800 //------------------------------------------------------------------------------------
1801 // called by fixAllSlips
1802 // assume there are no empty segments in the list
fixOneSlip(list<Segment>::iterator & kt,string which)1803 void GDCPass::fixOneSlip(list<Segment>::iterator& kt, string which)
1804 {
1805 try {
1806    if(kt->npts == 0) { kt++; return; }
1807 
1808    list<Segment>::iterator left,right;
1809 
1810    // kt points to the biggest segment
1811    // define left and right to be the two segments on each side of the slip to be
1812    // fixed; assume there are no empty segments in the list
1813    right = left = kt;
1814 
1815    // choose the next segment on the right of kt
1816    right++;
1817 
1818    // choose the next segment on the left of kt
1819    if(kt != SegList.begin())
1820       left--;
1821    else
1822       left = SegList.end();            // nothing on the left
1823 
1824    // no segment left of kt and no segment right of kt - nothing to do
1825    if(left == SegList.end() && right == SegList.end()) {
1826       kt = SegList.end();
1827       return;
1828    }
1829 
1830    // Always define kt to == left, as it will be returned and right will be erased.
1831    if(left == SegList.end()) {         // no segment on left
1832       left = kt;
1833    }
1834    else if(right == SegList.end()      // no segment on right
1835       || left->npts >= right->npts) {  // or left is the bigger segment
1836       right = kt;
1837       kt = left;                       // fix between left and kt
1838    }
1839    else {                              // left and right exist, and right is bigger
1840       left = kt;                       // fix between kt and right
1841    }
1842 
1843    // fix the slip between left and right, making data in 'right' part of 'left'
1844    if(which == string("WL"))
1845       WLslipFix(left,right);
1846    else
1847       GFslipFix(left,right);
1848 
1849    left->npts += right->npts;
1850    left->nend = right->nend;
1851 
1852    // always delete right, otherwise on return kt(==left) will be invalid
1853    // (ignore return value = iterator to first element after the one erased)
1854    SegList.erase(right);
1855 
1856    return;
1857 }
1858 catch(Exception& e) { GPSTK_RETHROW(e); }
1859 catch(std::exception& e) {
1860    Exception E("std except: "+string(e.what())); GPSTK_THROW(E);
1861 }
1862 catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
1863 }
1864 
1865 //------------------------------------------------------------------------------------
1866 // called by fixOneSlip
WLslipFix(list<Segment>::iterator & left,list<Segment>::iterator & right)1867 void GDCPass::WLslipFix(list<Segment>::iterator& left,
1868                         list<Segment>::iterator& right)
1869 {
1870 try {
1871    unsigned int i;
1872 
1873    GDCUniqueFix++;
1874 
1875    // full slip
1876    double dwl = right->bias1 + right->WLStats.Average()
1877          - (left->bias1 + left->WLStats.Average());
1878    long nwl = long(dwl + (dwl > 0 ? 0.5 : -0.5));
1879 
1880       // TD ? test gap size?
1881    //if(cfg(DT)*(right->nbeg - left->nend) > cfg(MaxGap)) break;
1882 
1883       // test that total variance is small
1884    //if(::sqrt(left->WLStats.Variance() + right->WLStats.Variance())
1885    //   / (left->WLStats.N() + right->WLStats.N()) < cfg(WLFixSigma)) {
1886    //   log << "Cannot fix WL slip (noisy) at " << right->nbeg
1887    //      << " " << printTime(time(right->nbeg),outFormat)
1888    //      << endl;
1889    //   break;
1890    //}
1891 
1892       // TD ? test fractional part of offset fabs
1893    //if(fabs(dwl-nwl) > cfg(WLFixFrac)) break;
1894 
1895    if(cfg(Debug) >= 6) log << "Fix " << GDCUnique << " " << sat << " " << GDCUniqueFix
1896       << " WL " << printTime(time(right->nbeg),outFormat)
1897       << " " << nwl           // put integer fix after time, all 'Fix'
1898       << " " << left->nseg << "-" << right->nseg
1899       << fixed << setprecision(2)
1900       << " right: " << right->bias1 << " + " << right->WLStats.Average()
1901       << " - left: " << left->bias1 << " + " << left->WLStats.Average()
1902       << " = " << dwl << " " << nwl
1903       << endl;
1904 
1905    // now do the fixing - change the data in the right segment to match left's
1906    for(i=right->nbeg; i<=right->nend; i++) {
1907       //if(!(spdvector[i].flag & OK)) continue;
1908       // 'change the data'
1909       spdvector[i].data[P1] -= nwl;                                 // WLbias
1910       spdvector[i].data[L2] -= nwl * wl2;                           // GFP
1911    }
1912 
1913    // fix the slips beyond the 'right' segment.
1914    // change the data in the GFP, and change both the data and the bias in the WL.
1915    // this way, WLStats is still valid, but if we change the GF bias, we will lose
1916    // that information before the GF slips get fixed.
1917    list<Segment>::iterator it = right;
1918    for(it++; it != SegList.end(); it++) {
1919       // Use real, not int, nwl b/c rounding error in a pass with many slips
1920       // can build up and produce errors.
1921       it->bias1 -= dwl;
1922       for(i=it->nbeg; i<=it->nend; i++) {
1923          spdvector[i].data[P1] -= nwl;                                 // WLbias
1924          spdvector[i].data[L2] -= nwl * wl2;                           // GFP
1925       }
1926    }
1927 
1928    // Add to slip list
1929    Slip newSlip(right->nbeg);
1930    newSlip.NWL = nwl;
1931    newSlip.msg = "WL";
1932    SlipList.push_back(newSlip);
1933 
1934    // mark it
1935    spdvector[right->nbeg].flag |= WLFIX;
1936 
1937    return;
1938 }
1939 catch(Exception& e) { GPSTK_RETHROW(e); }
1940 catch(std::exception& e) {
1941    Exception E("std except: "+string(e.what())); GPSTK_THROW(E);
1942 }
1943 catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
1944 }
1945 
1946 //------------------------------------------------------------------------------------
1947 // fix one slip in the geometry-free phase
1948 // called by fixOneSlip
GFslipFix(list<Segment>::iterator & left,list<Segment>::iterator & right)1949 void GDCPass::GFslipFix(list<Segment>::iterator& left,
1950                         list<Segment>::iterator& right)
1951 {
1952 try {
1953       // use this number of data points on each side of slip
1954    const unsigned int Npts=static_cast<unsigned int>(cfg(GFFixNpts));
1955    unsigned int i,nb,ne,nl,nr;
1956    int ilast;
1957    long n1,nadj;              // slip magnitude (cycles)
1958    double dn1,dnGFR;
1959    Stats<double> Lstats,Rstats;
1960 
1961    GDCUniqueFix++;
1962 
1963    // find Npts points on each side of slip
1964    nb = left->nend;
1965    i = 1;
1966    nl = 0;
1967    ilast = -1;                               // ilast is last good point before slip
1968    while(nb > left->nbeg && i < Npts) {
1969       if(spdvector[nb].flag & OK) {
1970          if(ilast == -1) ilast = nb;
1971          i++; nl++;
1972          Lstats.Add(spdvector[nb].data[L1] - left->bias2);
1973          //log << "LDATA " << nb << " " << spdvector[nb].data[L1]-left->bias2 << endl;
1974       }
1975       nb--;
1976    }
1977    ne = right->nbeg;
1978    i = 1;
1979    nr = 0;
1980    while(ne < right->nend && i < Npts) {
1981       if(spdvector[ne].flag & OK) {
1982          i++; nr++;
1983          Rstats.Add(spdvector[ne].data[L1] - right->bias2);
1984          //log << "RDATA " << ne << " " << spdvector[ne].data[L1]-right->bias2 <<endl;
1985       }
1986       ne++;
1987    }
1988 
1989    // first estimate of n1, without biases
1990    // need to use the GFR-GFP estimate here, and limit |nadj| to be well within
1991    // sigmas on the stats, b/c when ionosphere is very active, GFP and GFR will both
1992    // vary sharply, and fitting a polynomial to GFP is a BAD thing to do....
1993    // ultimately, GFR-GFP is accurate but noisy.
1994    // rms rof should tell you how much weight to put on rof
1995    // larger rof -> smaller npts and larger degree
1996    dn1 = spdvector[right->nbeg].data[L2] - right->bias2
1997          - (spdvector[ilast].data[L2] - left->bias2);
1998    n1 = long(dn1 + (dn1 > 0 ? 0.5 : -0.5));
1999 
2000    // estimate the slip using polynomial fits - this prints GFE data
2001    nadj = EstimateGFslipFix(left,right,nb,ne,n1);
2002 
2003    // adjust the adjustment if it is not consistent with Lstats vs Rstats
2004    // dn1+nadj                       - a. current best estimate
2005    // Rstats.Averge()-Lstats.Average - b. estimate from stats on GFR-GFP across slip
2006    // difference should be consistent with R/Lstats.StdDev
2007    // if not, replace nadj with b. - dn1
2008    dnGFR = Rstats.Average() - Lstats.Average();
2009    if(fabs(n1+nadj-dnGFR) > 10.*(Rstats.StdDev()+Lstats.StdDev())) {
2010       if(cfg(Debug) >= 6)
2011          log << "GFRadjust " << GDCUnique << " " << sat << " " << GDCUniqueFix
2012          << " GF " << printTime(time(right->nbeg),outFormat)
2013          << fixed << setprecision(2)
2014          << " dbias(GFR): " << dnGFR
2015          << " n1+nadj: " << n1+nadj;
2016 
2017       nadj = long(dnGFR+(dnGFR > 0 ? 0.5 : -0.5)) - n1;
2018 
2019       if(cfg(Debug) >= 6)
2020          log << " new n1+nadj: " << n1+nadj << endl;
2021    }
2022 
2023    // output result
2024    if(cfg(Debug) >= 6) {
2025       log << "Fix " << GDCUnique << " " << sat << " " << GDCUniqueFix
2026       << " GF " << printTime(time(right->nbeg),outFormat)
2027       << " " << nadj           // put integer fix after time, all 'Fix'
2028       << fixed << setprecision(2)
2029       << " dbias: " << right->bias2 - left->bias2
2030       << ", dn1: " << dn1 << ", n1: " << n1 << ", adj: " << nadj
2031       << " indexes " << nb << " " << ne << " " << nl << " " << nr
2032       << " segs " << left->nseg << " " << right->nseg
2033       << " GFR-GFP:L: "
2034       << Lstats.N() << " " << Lstats.Average() << " " << Lstats.StdDev()
2035       << "    R: "
2036       << Rstats.N() << " " << Rstats.Average() << " " << Rstats.StdDev()
2037       << " tests " << n1+nadj-dnGFR << " " << Rstats.StdDev()+Lstats.StdDev()
2038       << endl;
2039    }
2040 
2041    // full slip, including biases
2042    dn1 += right->bias2 - left->bias2;
2043    n1 = long(dn1 + (dn1 > 0 ? 0.5 : -0.5));
2044    n1 += nadj;
2045 
2046    // now do the fixing : 'change the data' within right segment
2047    // and through the end of the pass, to fix the slip
2048    for(i=right->nbeg; i<static_cast<int>(size()); i++) {
2049       spdvector[i].data[L2] -= n1;                              // GFP
2050       spdvector[i].data[L1] -= n1;                              // GFR+GFP
2051    }
2052 
2053    // 'change the bias' for all segments in the future (although right to be deleted)
2054    list<Segment>::iterator kt;
2055    for(kt=right; kt != SegList.end(); kt++)
2056       kt->bias2 -= n1;
2057 
2058    // Add to slip list, but if one exists with same time tag, use it instead
2059    list<Slip>::iterator jt;
2060    for(jt=SlipList.begin(); jt != SlipList.end(); jt++)
2061       if(jt->index == right->nbeg) break;
2062 
2063    if(jt == SlipList.end()) {
2064       Slip newSlip(right->nbeg);
2065       newSlip.N1 = -n1;
2066       newSlip.msg = "GF only";
2067       SlipList.push_back(newSlip);
2068    }
2069    else {
2070       jt->N1 = -n1;
2071       jt->msg += string(" GF");
2072    }
2073 
2074    // mark it
2075    spdvector[right->nbeg].flag |= GFFIX;
2076 
2077    return;
2078 }
2079 catch(Exception& e) { GPSTK_RETHROW(e); }
2080 catch(std::exception& e) {
2081    Exception E("std except: "+string(e.what())); GPSTK_THROW(E);
2082 }
2083 catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
2084 }
2085 
2086 //------------------------------------------------------------------------------------
2087 // called by GFslipFix
2088 // estimate GF slip using polynomial fit to data around it
EstimateGFslipFix(list<Segment>::iterator & left,list<Segment>::iterator & right,unsigned int nb,unsigned int ne,long n1)2089 long GDCPass::EstimateGFslipFix(list<Segment>::iterator& left,
2090                                list<Segment>::iterator& right,
2091                                unsigned int nb, unsigned int ne, long n1)
2092 {
2093 try {
2094    bool quit;
2095    unsigned int i,k,in[3];
2096    double rof,rmsrof[3];
2097    PolyFit<double> PF[3];
2098 
2099    // start at zero and limit |nadj| to ...TD
2100    long nadj(0);
2101 
2102    // use a little indirect indexing array to avoid having to copy PolyFit objects....
2103    for(k=0; k<3; k++) {
2104       in[k]=k;
2105       PF[in[k]].Reset(int(cfg(GFFixDegree)));
2106    }
2107 
2108    while(1) {
2109       // compute 3 polynomial fits to this data, with slips of
2110       // (nadj-1, nadj and nadj+1) wavelengths added to left segment
2111       for(k=0; k<3; k++) {
2112          if(PF[in[k]].N() > 0) continue;
2113 
2114          // add all the data
2115          for(i=nb; i<=ne; i++) {
2116             if(!(spdvector[i].flag & OK)) continue;
2117             PF[in[k]].Add(
2118                // data
2119                spdvector[i].data[L2]
2120                // - (either               left bias - poss. slip : right bias)
2121                   - (i < right->nbeg ? left->bias2-n1-(nadj+k-1) : right->bias2),
2122                //  use a debiased count
2123                spdvector[i].ndt - spdvector[nb].ndt
2124             );
2125          }
2126 
2127          // TD check that it not singular
2128 
2129          // compute RMS residual of fit
2130          rmsrof[in[k]] = 0.0;
2131          for(i=nb; i<=ne; i++) {
2132             if(!(spdvector[i].flag & OK)) continue;
2133             rof =    // data minus fit
2134                spdvector[i].data[L2]
2135                   - (i < right->nbeg ? left->bias2-n1-(nadj+k-1) : right->bias2)
2136                - PF[in[k]].Evaluate(spdvector[i].ndt - spdvector[nb].ndt);
2137             rmsrof[in[k]] += rof*rof;
2138          }
2139          rmsrof[in[k]] = ::sqrt(rmsrof[in[k]]);
2140 
2141       }  // end loop over fits
2142 
2143       // the value of this is questionable, b/c with active ionosphere the real
2144       // GFP is NOT smooth
2145       for(quit=false,k=0; k<3; k++) if(rmsrof[in[k]] > cfg(GFFixMaxRMS)) {
2146          log << "Warning - large RMS ROF in GF slip fix at in,k = "
2147             << in[k] << " " << k << " " << rmsrof[in[k]] << " abort.\n";
2148          quit = true;
2149       }
2150       if(quit) break;
2151 
2152       // three cases: (TD - exceptions?) :
2153       // rmsrof: 0 > 1 < 2   good
2154       //         0 > 1 > 2   shift 0,1,2 to 1,2,3
2155       //         0 < 1 < 2   shift 0,1,2 to -1,0,1
2156       //         0 < 1 > 2   local max! - ??
2157       if(rmsrof[in[0]] > rmsrof[in[1]]) {
2158          if(rmsrof[in[1]] < rmsrof[in[2]]) { // local min - done
2159             //if(cfg(Debug) >= 6) log << " done." << endl;
2160             break;
2161          }
2162          else {                              // shift 0,1,2 to 1,2,3
2163             k = in[0];
2164             in[0] = in[1];
2165             in[1] = in[2];
2166             in[2] = k;
2167             PF[in[2]].Reset();
2168             nadj += 1;
2169             //if(cfg(Debug) >= 6) log << " shift left" << endl;
2170          }
2171       }
2172       else {
2173          if(rmsrof[in[1]] < rmsrof[in[2]]) { // shift 0,1,2 to -1,0,1
2174             k = in[2];
2175             in[2] = in[1];
2176             in[1] = in[0];
2177             in[0] = k;
2178             PF[in[0]].Reset();
2179             nadj -= 1;
2180             //if(cfg(Debug) >= 6) log << " shift right" << endl;
2181          }
2182          else {                              // local max
2183             log << "Warning - local maximum in RMS residuals in EstimateGFslipFix"
2184                << endl;
2185             // TD do something
2186             break;
2187          }
2188       }
2189 
2190    }  // end while loop
2191 
2192    // dump the raw data with all the fits
2193    if(cfg(Debug) >= 4) {
2194       log << "EstimateGFslipFix dump " << endl;
2195       for(i=nb; i<=ne; i++) {
2196          if(!(spdvector[i].flag & OK)) continue;
2197          log << "GFE " << GDCUnique << " " << sat
2198             << " " << GDCUniqueFix
2199             << " " << printTime(time(i),outFormat)
2200             << " " << setw(2) << spdvector[i].flag << fixed << setprecision(3);
2201          for(k=0; k<3; k++) log << " " << spdvector[i].data[L2]
2202                - (i < right->nbeg ? left->bias2-n1-(nadj+k-1) : right->bias2)
2203             << " " << PF[in[k]].Evaluate(spdvector[i].ndt - spdvector[nb].ndt);
2204          log << " " << setw(3) << spdvector[i].ndt << endl;
2205       }
2206    }
2207 
2208    return nadj;
2209 }
2210 catch(Exception& e) { GPSTK_RETHROW(e); }
2211 catch(std::exception& e) {
2212    Exception E("std except: "+string(e.what())); GPSTK_THROW(E);
2213 }
2214 catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
2215 }
2216 
2217 //------------------------------------------------------------------------------------
2218 //------------------------------------------------------------------------------------
2219 // 07202010 not used fit a polynomial to the GF range,
2220 // change the units of -gfr(P2) and gfp(L2) to cycles of wlgf (=5.4cm)
prepareGFdata(void)2221 int GDCPass::prepareGFdata(void)
2222 {
2223 try {
2224    bool first;
2225    int i,nbeg,nend;
2226 
2227    // decide on the degree of fit
2228    nbeg = SegList.begin()->nbeg;
2229    nend = SegList.begin()->nend;
2230 
2231    for(first=true,i=nbeg; i <= nend; i++) {
2232       if(!(spdvector[i].flag & OK)) continue;
2233 
2234       // 'change the bias' (initial bias only) in the GFP by changing units, also
2235       // slip fixing in the WL may have changed the values of GFP
2236       if(first) {
2237          SegList.begin()->bias2 /= wlgf;
2238          first = false;
2239       }
2240 
2241       // 'change the arrays'
2242       // change units on the GFP and the GFR
2243       spdvector[i].data[P2] /= wlgf;                    // -gfr (cycles of wlgf)
2244       spdvector[i].data[L2] /= wlgf;                    // gfp (cycles of wlgf)
2245 
2246       // 'change the data'
2247       // save in L1                          // gfp+gfr residual (cycles of wlgf)
2248       spdvector[i].data[L1] = spdvector[i].data[L2] - spdvector[i].data[P2];
2249    }
2250 
2251    return ReturnOK;
2252 }
2253 catch(Exception& e) { GPSTK_RETHROW(e); }
2254 catch(std::exception& e) {
2255    Exception E("std except: "+string(e.what())); GPSTK_THROW(E);
2256 }
2257 catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
2258 }
2259 
2260 //------------------------------------------------------------------------------------
2261 //------------------------------------------------------------------------------------
2262 // detect slips in the geometry-free phase
detectGFslips(void)2263 int GDCPass::detectGFslips(void)
2264 {
2265 try {
2266    unsigned int i;
2267    int iret;
2268    list<Segment>::iterator it;
2269 
2270    // places first difference of GF in A1 - 'change the arrays' A1
2271    if( (iret = detectObviousSlips("GF"))) return iret;
2272 
2273    GFPassStats.Reset();
2274    for(it=SegList.begin(); it != SegList.end(); it++) {
2275       // compute stats on dGF/dt
2276       for(i=it->nbeg; i <= it->nend; i++) {
2277          if(!(spdvector[i].flag & OK)) continue;
2278 
2279          // compute first-diff stats in meters
2280          // skip the first point in a segment - it is an obvious GF slip
2281          if(i > it->nbeg) GFPassStats.Add(spdvector[i].data[A1]*wlgf);
2282 
2283       }  // end loop over data in segment it
2284 
2285       // check number of good points
2286       if(it->npts < static_cast<unsigned int>(cfg(MinPts))) {
2287          deleteSegment(it,"insufficient data in segment");
2288          continue;
2289       }
2290 
2291       // fit polynomial to GFR in each segment
2292       // compute (1stD of) fit residual GFP-fit(GFR) -> A1 - 'change the arrays' A1
2293       // delete segment if polynomial is singular - probably due to too little data
2294       if( (iret = GFphaseResiduals(it))) {
2295          deleteSegment(it,"polynomial fit to GF residual failed");
2296          continue;
2297       }
2298    }
2299 
2300    // 'change the arrays'
2301    // at this point:
2302    // L1 = GFP+GFR in cycles, by prepareGFdata()
2303    // L2 = GFP in cycles, by prepareGFdata()
2304    // P1 = wlbias
2305    // P2 = -GFR in cycles, by prepareGFdata()
2306    // A1 = GFP-(local fit) OR its 1stD, by GFphaseResiduals()
2307    //      (was 1stD of GFP+GFR (in L1), by firstDifferences())
2308    // A2 = 1stD of GFP (in L2), by firstDifferences()
2309    if( (iret = detectGFsmallSlips())) return iret;
2310 
2311    // delete all segments that are too small
2312    for(it=SegList.begin(); it != SegList.end(); it++) {
2313       if(it->npts < static_cast<unsigned int>(cfg(MinPts)))
2314          deleteSegment(it,"insufficient data in segment");
2315    }
2316 
2317    if(cfg(Debug) >= 4) dumpSegments("GFD",2,true);
2318 
2319    return ReturnOK;
2320 }
2321 catch(Exception& e) { GPSTK_RETHROW(e); }
2322 catch(std::exception& e) {
2323    Exception E("std except: "+string(e.what())); GPSTK_THROW(E);
2324 }
2325 catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
2326 }
2327 
2328 //------------------------------------------------------------------------------------
2329 // for each segment, fit a polynomial to the gfr, then compute and store the
2330 // residual of fit
GFphaseResiduals(list<Segment>::iterator & it)2331 int GDCPass::GFphaseResiduals(list<Segment>::iterator& it)
2332 {
2333 try {
2334    unsigned int i;
2335    int ndeg,nprev;
2336    double fit,rbias,prev,tmp;
2337    Stats<double> rofStats;
2338 
2339    // decide on the degree of fit
2340    ndeg = 2 + int(0.5 + (it->nend-it->nbeg+1)*cfg(DT)/3000.0);
2341    //if(ndeg > int(cfg(GFPolyMaxDegree))) ndeg = int(cfg(GFPolyMaxDegree));
2342    if(ndeg > 6) ndeg = 6;
2343    if(ndeg < 2) ndeg = 2;
2344 
2345    it->PF.Reset(ndeg);     // for fit to GF range
2346 
2347    for(i=it->nbeg; i <= it->nend; i++) {
2348       if(!(spdvector[i].flag & OK)) continue;
2349       it->PF.Add(spdvector[i].data[P2],spdvector[i].ndt);
2350    }
2351 
2352    if(it->PF.isSingular()) {     // this should never happen
2353       log << "Polynomial fit to GF range is singular in segment " << it->nseg
2354          << "! .. abort." << endl;
2355       return Singular;
2356    }
2357 
2358    // now compute the residual of fit
2359    rbias = prev = 0.0;
2360    rofStats.Reset();
2361    for(i=it->nbeg; i <= it->nend; i++) {
2362       // skip bad data
2363       if(!(spdvector[i].flag & OK)) continue;
2364 
2365       fit = it->PF.Evaluate(spdvector[i].ndt);
2366 
2367       // all (fit, resid, gfr and gfp) are in cycles of wlgf (5.4cm)
2368 
2369       // compute gfp-(fit to gfr), store in A1 - 'change the arrays' A1 and A2
2370       // OR let's try first difference of residual of fit
2371       //           residual =  phase                            - fit to range
2372       spdvector[i].data[A1] = spdvector[i].data[L2] - it->bias2 - fit;
2373       if(rbias == 0.0) {
2374          rbias = spdvector[i].data[A1];
2375          nprev = spdvector[i].ndt - 1;
2376       }
2377       spdvector[i].data[A1] -= rbias;                    // debias residual for plots
2378 
2379          // compute stats on residual of fit
2380       rofStats.Add(spdvector[i].data[A1]);
2381 
2382       if(1) { // 1stD of residual - remember A1 has just been debiased
2383          tmp = spdvector[i].data[A1];
2384          spdvector[i].data[A1] -= prev;       // diff with previous epoch's
2385          // 040809 should this be divided by delta n?
2386          // spdvector[i].data[A1] /= (spdvector[i].ndt - nprev);
2387          prev = tmp;          // store residual for next point
2388          nprev = spdvector[i].ndt;
2389       }
2390 
2391    }
2392 
2393    return ReturnOK;
2394 }
2395 catch(Exception& e) { GPSTK_RETHROW(e); }
2396 catch(std::exception& e) {
2397    Exception E("std except: "+string(e.what())); GPSTK_THROW(E);
2398 }
2399 catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
2400 }
2401 
2402 //------------------------------------------------------------------------------------
2403 // detect small slips in the geometry-free phase
2404 // TD outliers at the beginning or end of the segment....
detectGFsmallSlips(void)2405 int GDCPass::detectGFsmallSlips(void)
2406 {
2407 try {
2408    const unsigned int width=static_cast<unsigned int>(cfg(GFSlipWidth));
2409    int i,j,iplus,inew,ifirst,nok;
2410    list<Segment>::iterator it;
2411    Stats<double> pastStats,futureStats;
2412 
2413    // loop over segments
2414    for(it=SegList.begin(); it != SegList.end(); it++) {
2415 
2416       if(it->npts < 2*width+1) continue;
2417 
2418       // Cartoon of the GF 'two-pane moving window'
2419       //          point of interest:|
2420       // windows:     'past window' | 'future window'
2421       // stats  :        pastStats  | futureStats  (5 pts in each window)
2422       // data   : ... x (x x x x x) x (x x x x x) x ...
2423       //                 |          |          |
2424       // indexes:        j          i        iplus
2425 
2426       deque<int> pastIndex, futureIndex;
2427       pastStats.Reset();
2428       futureStats.Reset();
2429       i = inew = ifirst = -1;
2430       nok = 0;                          // recount
2431 
2432       // loop over points in the segment
2433       for(iplus=static_cast<int>(it->nbeg);
2434          iplus<=static_cast<int>(it->nend+width);
2435             iplus++)
2436       {
2437          // ignore bad points
2438          if(iplus <= static_cast<int>(it->nend) && !(spdvector[iplus].flag & OK))
2439             continue;
2440          if(ifirst == -1) ifirst = iplus;
2441 
2442          // pop the new i from the future
2443          if(static_cast<int>(futureIndex.size()) == width
2444                   || iplus > static_cast<int>(it->nend))
2445          {
2446             inew = futureIndex.front();
2447             futureIndex.pop_front();
2448             futureStats.Subtract(spdvector[inew].data[A1]);
2449             nok++;
2450          }
2451 
2452          // put iplus into the future deque
2453          if(iplus <= static_cast<int>(it->nend)) {
2454             futureIndex.push_back(iplus);
2455             futureStats.Add(spdvector[iplus].data[A1]);
2456          }
2457          else
2458             futureIndex.push_back(-1);
2459 
2460          // check for outliers
2461          // we now have:
2462          //                (  past   )     ( future  )
2463          // data   : ... x (x x x x x) x x (x x x x x) x ...
2464          //                            | |          |
2465          // indexes:                   i inew     iplus
2466          // outlier if: (i,inew) = opposite signs but ~= large magnitude
2467          // if found, mark i bad and replace A1(inew) = A1(inew)+A1(i)
2468          if(foundGFoutlier(i,inew,pastStats,futureStats)) {
2469             // check that i was not marked a slip in the last iteration
2470             // if so, let inew be the slip and i the outlier
2471             if(spdvector[i].flag & DETECT) {
2472                //log << "Warning - marking a slip point BAD in GF detect small "
2473                //   << GDCUnique << " " << sat
2474                //   << " " << printTime(time(i),outFormat) << " " << i << endl;
2475                spdvector[inew].flag = spdvector[i].flag;
2476                it->nbeg = inew;
2477             }
2478             spdvector[i].flag = BAD;
2479             spdvector[inew].data[A1] += spdvector[i].data[A1];
2480             learn["points deleted: GF outlier"]++;
2481             i = inew;
2482             nok--;
2483          }
2484 
2485          // pop last from past
2486          if(static_cast<int>(pastIndex.size()) == width) {
2487             j = pastIndex.front();
2488             pastIndex.pop_front();
2489             pastStats.Subtract(spdvector[j].data[A1]);
2490          }
2491 
2492          // move i into the past
2493          if(i > -1) {
2494             pastIndex.push_back(i);
2495             pastStats.Add(spdvector[i].data[A1]);
2496          }
2497 
2498          // return to original state
2499          i = inew;
2500 
2501          // test for slip .. foundGF...prints to log
2502          if(foundGFsmallSlip(i,it->nseg,it->nend,it->nbeg,
2503             pastIndex,futureIndex,pastStats,futureStats)) {
2504 
2505             // create a new segment
2506             it->npts = nok-1;
2507             it = createSegment(it,i,"GF slip small");
2508             nok = 1;
2509 
2510             // mark it
2511             spdvector[i].flag |= GFDETECT;
2512          }
2513 
2514       }  // end loop over points in the pass
2515       it->npts = nok;
2516 
2517    }  // end loop over segments
2518 
2519    return ReturnOK;
2520 }
2521 catch(Exception& e) { GPSTK_RETHROW(e); }
2522 catch(std::exception& e) {
2523    Exception E("std except: "+string(e.what())); GPSTK_THROW(E);
2524 }
2525 catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
2526 }
2527 
2528 //------------------------------------------------------------------------------------
foundGFoutlier(int i,int inew,Stats<double> & pastSt,Stats<double> & futureSt)2529 bool GDCPass::foundGFoutlier(int i, int inew,
2530    Stats<double>& pastSt, Stats<double>& futureSt)
2531 {
2532 try {
2533    if(i < 0 || inew < 0) return false;
2534    bool ok;
2535    double pmag = spdvector[i].data[A1]; // -pastSt.Average();
2536    double fmag = spdvector[inew].data[A1]; // -futureSt.Average();
2537    double var = ::sqrt(pastSt.Variance() + futureSt.Variance());
2538 
2539    ostringstream oss;
2540    if(cfg(Debug) >= 6) oss << "GFoutlier " << GDCUnique
2541       << " " << sat << " " << setw(3) << inew
2542       << " " << printTime(time(inew),outFormat)
2543       << fixed << setprecision(3)
2544       << " p,fave=" << fabs(pmag) << "," << fabs(fmag)
2545       << " var=" << var
2546       << " snr=" << fabs(pmag)/var <<","<< fabs(fmag)/var;
2547 
2548    bool isOut=true;
2549    for(;;) {
2550 
2551       // 1. signs must be opposite
2552       if(pmag * fmag >= 0) isOut=false;
2553       if(cfg(Debug) >= 6) oss << " (1)" << (isOut?"ok":"no");
2554       if(!isOut) break;
2555 
2556       // 2. magnitudes must be large compared to noise
2557       double noise=cfg(GFSlipOutlier)*var;
2558       if(fabs(pmag) < noise || fabs(fmag) < noise) isOut=false;
2559       if(cfg(Debug) >= 6) oss << " (2)" << fabs(pmag)/var << "or" << fabs(fmag)/var
2560          << (isOut?">=":"<") << cfg(GFSlipOutlier);
2561       if(!isOut) break;
2562 
2563       if(cfg(Debug) >= 6) oss << " possible GF outlier";
2564 
2565       break;
2566    }  // end for(;;)
2567 
2568    if(cfg(Debug) >= 6) log << oss.str() << endl;
2569 
2570    return isOut;
2571 }
2572 catch(Exception& e) { GPSTK_RETHROW(e); }
2573 catch(std::exception& e) {
2574    Exception E("std except: "+string(e.what())); GPSTK_THROW(E);
2575 }
2576 catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
2577 }
2578 
2579 //------------------------------------------------------------------------------------
2580 // Better to find too many small ones than to miss them, since the fixing algorithm
2581 // will most likely refuse to act on the questionable ones.
foundGFsmallSlip(int i,int nseg,int iend,int ibeg,deque<int> & pastIn,deque<int> & futureIn,Stats<double> & pastSt,Stats<double> & futureSt)2582 bool GDCPass::foundGFsmallSlip(int i,int nseg,int iend,int ibeg,
2583    deque<int>& pastIn,deque<int>& futureIn,
2584    Stats<double>& pastSt, Stats<double>& futureSt)
2585 {
2586 try {
2587    if(i < 0) return false;
2588 
2589    int j,k;
2590    double mag,pmag,fmag,pvar,fvar;
2591 
2592    pmag = fmag = pvar = fvar = 0.0;
2593    // note when past.N == 1, this is first good point, which has 1stD==0
2594    // TD be very careful when N is small
2595    if(pastSt.N() > 0) pmag = spdvector[i].data[A1]-pastSt.Average();
2596    if(futureSt.N() > 0) fmag = spdvector[i].data[A1]-futureSt.Average();
2597    if(pastSt.N() > 1) pvar = pastSt.Variance();
2598    if(futureSt.N() > 1) fvar = futureSt.Variance();
2599    mag = (pmag + fmag) / 2.0;
2600 
2601    if(cfg(Debug) >= 6) log << "GFS " << GDCUnique
2602       << " " << sat << " " << nseg
2603       << " " << printTime(time(i),outFormat)
2604       //<< " P( " << setw(3) << pastIn[0]            // don't print this...
2605       //<< " " << setw(3) << pastIn[1]
2606       //<< " " << setw(3) << pastIn[2]
2607       //<< " " << setw(3) << pastIn[3]
2608       //<< " " << setw(3) << pastIn[4]
2609       //<< ") " << setw(3) << i
2610       //<< " F( " << setw(3) << futureIn[0]
2611       //<< " " << setw(3) << futureIn[1]
2612       //<< " " << setw(3) << futureIn[2]
2613       //<< " " << setw(3) << futureIn[3]
2614       //<< " " << setw(3) << futureIn[4] << ")"       // ...to here
2615       << fixed << setprecision(3)
2616       << " " << setw(3) << pastSt.N()
2617       << " " << setw(7) << pastSt.Average()
2618       << " " << setw(7) << pastSt.StdDev()
2619       << " " << setw(3) << futureSt.N()
2620       << " " << setw(7) << futureSt.Average()
2621       << " " << setw(7) << futureSt.StdDev()
2622       << " " << setw(7) << mag
2623       << " " << setw(7) << ::sqrt(pvar+fvar)
2624       << " " << setw(9) << spdvector[i].data[A1]
2625       << " " << setw(7) << pmag
2626       << " " << setw(7) << pvar
2627       << " " << setw(7) << fmag
2628       << " " << setw(7) << fvar
2629       << " " << setw(3) << i
2630       << endl;
2631 
2632    //                    x                    -- mag
2633    //
2634    //    x   x   x   x                         - step
2635    //                       x    x   x   x   ---
2636    const double minMag=cfg(GFSlipSize);     // minimum slip magnitude
2637    const double STN=cfg(GFSlipStepToNoise); // step (past->future) to noise ratio
2638    const double MTS=cfg(GFSlipToStep);      // magnitude to step ratio
2639    const double MTN=cfg(GFSlipToNoise);     // magnitude to noise ratio
2640    const int Edge=int(cfg(GFSlipEdge));     // number of points before edge
2641    const double RangeCheckLimit = 2*cfg(WLSigma)/(0.83*wlgf);
2642                                             // 2 * range noise in units of wlgf
2643    const double snr=fabs(pmag-fmag)/::sqrt(pvar+fvar);
2644                                             // slip to noise ratio
2645 
2646    // 050109 if Debug=6, print only possible slips, if 7 print all
2647    bool isSlip=true;
2648    ostringstream oss;
2649 
2650    for(;;) {
2651       // NB last printed test is a failure unless it says possible GF slip
2652       if(cfg(Debug) >= 6) oss << "GFslip " << GDCUnique
2653          << " " << sat << " " << nseg
2654          << " " << setw(3) << i
2655          << " " << printTime(time(i),outFormat) << fixed << setprecision(3)
2656          << " mag=" << mag << " snr=" << snr;      // no endl
2657 
2658       // 0. if WL slip here - ...?
2659 
2660       // 1. slip must be non-trivial
2661       if(fabs(mag) <= minMag) isSlip=false;
2662       if(cfg(Debug) >= 6) oss << " (1)|" << mag << (isSlip?"|>":"|<=") << minMag;
2663       if(!isSlip) break;
2664 
2665       // 2. change in average is larger than noise
2666       if(snr <= STN) isSlip=false;
2667       if(cfg(Debug) >= 6) oss << " (2)" << snr << (isSlip?">":"<=") << STN;
2668       if(!isSlip) break;
2669 
2670       // 3. slip is large compared to change in average
2671       if(fabs(mag) <= MTS*fabs(pmag-fmag)) isSlip=false;
2672       if(cfg(Debug) >= 6) oss << " (3)" << fabs(mag/(pmag-fmag))
2673          << (isSlip?">":"<=") << MTS;
2674       if(!isSlip) break;
2675 
2676       // 4. magnitude is large compared to noise: a 3-sigma slip
2677       if(fabs(mag) <= MTN*::sqrt(pvar+fvar)) isSlip=false;
2678       if(cfg(Debug) >= 6) oss << " (4)" << fabs(mag)/::sqrt(pvar+fvar)
2679          << (isSlip?">":"<=") << MTN;
2680       if(!isSlip) break;
2681 
2682       // 5. if very close to edge, declare it an outlier
2683       if(static_cast<int>(pastSt.N()) < Edge
2684          || static_cast<int>(futureSt.N()) < Edge+1) isSlip=false;
2685       if(cfg(Debug) >= 6) oss << " (5)" << pastSt.N() << "," << futureSt.N()
2686          << (isSlip?">":"<") << Edge;
2687       if(!isSlip) break;
2688 
2689       // 6. large slips (compared to range noise): check the GFR-GFP for consistency
2690       if(fabs(mag) > RangeCheckLimit) {
2691          double magGFR,mtnGFR;
2692          Stats<double> pGFRmPh,fGFRmPh;
2693          for(j=0; j<static_cast<int>(pastIn.size()); j++) {
2694             if(pastIn[j] > -1) pGFRmPh.Add(spdvector[pastIn[j]].data[L1]);
2695             if(futureIn[j] > -1) fGFRmPh.Add(spdvector[futureIn[j]].data[L1]);
2696          }
2697          magGFR = fGFRmPh.Average() - pGFRmPh.Average();
2698          mtnGFR = fabs(magGFR)/::sqrt(pGFRmPh.Variance()+fGFRmPh.Variance());
2699 
2700          if(cfg(Debug) >= 6)
2701             oss << "; GFR-GFP has mag: " << magGFR
2702                << ", |dmag|: " << fabs(mag-magGFR)
2703                << " and mag/noise " << mtnGFR;
2704 
2705          if(fabs(mag-magGFR) > fabs(magGFR)) isSlip=false;
2706          if(cfg(Debug) >= 6) oss << " (6a)" << fabs(mag-magGFR)
2707             << (isSlip ? "<=" : ">") << fabs(magGFR);
2708          if(!isSlip) break;
2709 
2710          if(mtnGFR < 3) isSlip=false;
2711          if(cfg(Debug) >= 6) oss << " (6b)"
2712             << mtnGFR << "><3:can" << (isSlip ? "" : "not") << "_see_in_GFR";
2713          if(!isSlip) break;
2714       }
2715 
2716       // 7. small slips (compared to variations in dGF): extra careful
2717       // TD beware of small slips in the presence of noise >~ 1
2718       else { //if(fabs(mag) <= RangeCheckLimit)
2719          double magFD;
2720          Stats<double> fdStats;
2721          j = i-1; k=0;
2722          while(j >= ibeg && k < 15) {
2723             if(spdvector[j].flag & OK) { fdStats.Add(spdvector[j].data[A2]); k++; }
2724             j--;
2725          }
2726          j = i+1; k=0;
2727          while(j <= iend && k < 15) {
2728             if(spdvector[j].flag & OK) { fdStats.Add(spdvector[j].data[A2]); k++; }
2729             j++;
2730          }
2731          magFD = spdvector[i].data[A2] - fdStats.Average();
2732 
2733          if(cfg(Debug) >= 6)
2734             oss << " (7)1stD(GFP)mag=" << magFD
2735                << ",noise=" << fdStats.StdDev()
2736                << ",snr=" << fabs(magFD)/fdStats.StdDev()
2737                << ",maxima=" << fdStats.Minimum() << "," << fdStats.Maximum();
2738       }
2739 
2740       // 8. if switch is on and there is no WL slip here - skip
2741       if(cfg(GFSkipSmall) && !(spdvector[i].flag & WLDETECT)) {
2742          if(cfg(Debug) >= 6) oss << " (8)skipGFsmall";
2743          isSlip = false;
2744       }
2745 
2746       break;
2747    }  // end for(;;)
2748 
2749    if(isSlip) oss << " possible GF slip"; else oss << " not a GF slip";
2750    if(cfg(Debug) >= 6) log << oss.str() << endl;
2751 
2752    return isSlip;
2753 }
2754 catch(Exception& e) { GPSTK_RETHROW(e); }
2755 catch(std::exception& e) {
2756    Exception E("std except: "+string(e.what())); GPSTK_THROW(E);
2757 }
2758 catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
2759 }
2760 
2761 //------------------------------------------------------------------------------------
2762 //------------------------------------------------------------------------------------
2763 // check the consistency of WL slips where a GF slip, but not a WL slip, was detected.
WLconsistencyCheck(void)2764 int GDCPass::WLconsistencyCheck(void)
2765 {
2766 try {
2767    int i,k;
2768    const int N=2*int(cfg(WLWindowWidth));
2769    double mag,absmag,factor=wl2/wlgf;
2770 
2771    // loop over the data and look for points with GFDETECT but not WLDETECT or WLFIX
2772    for(i=0; i<static_cast<int>(size()); i++) {
2773 
2774       if(!(spdvector[i].flag & OK)) continue;        // bad
2775       if(!(spdvector[i].flag & DETECT)) continue;    // no slips
2776       if(spdvector[i].flag & WLDETECT) continue;     // WL was detected
2777 
2778       // GF only slip - compute WL stats on both sides
2779       Stats<double> futureStats,pastStats;
2780       k = i;
2781       // fill future
2782       while(k < static_cast<int>(size()) && static_cast<int>(futureStats.N()) < N) {
2783          if(spdvector[k].flag & OK)                  // data is good
2784             futureStats.Add(spdvector[k].data[P1]);        // wlbias
2785          k++;
2786       }
2787       // fill past
2788       k = i-1;
2789       while(k >= 0 && static_cast<int>(pastStats.N()) < N) {
2790          if(spdvector[k].flag & OK)                  // data is good
2791             pastStats.Add(spdvector[k].data[P1]);          // wlbias
2792          k--;
2793       }
2794 
2795       // is there a WL slip here?
2796       // 1. magnitude of slip > 0.75
2797       // 2. magnitude is > stddev on both sides
2798       // 3. N() > 10 on both sides TD??
2799       mag = futureStats.Average()-pastStats.Average();
2800       absmag = fabs(mag);
2801 
2802       if(absmag > cfg(WLSlipSize) &&
2803          absmag > pastStats.StdDev() &&
2804          absmag > futureStats.StdDev()) {
2805 
2806          long nwl;
2807          nwl = long(mag + (mag > 0 ? 0.5 : -0.5));
2808 
2809          if(nwl == 0) continue;
2810 
2811          // now do the fixing - change the data to the future of the slip
2812          for(k=i; k<static_cast<int>(size()); k++) {
2813             //if(!(spdvector[i].flag & OK)) continue;
2814             // 'change the data'
2815             spdvector[k].data[P1] -= nwl;                                 // WLbias
2816             spdvector[k].data[L2] -= nwl * factor;                        // GFP
2817          }
2818 
2819          // Add to slip list
2820          Slip newSlip(i);
2821          newSlip.NWL = nwl;
2822          newSlip.msg = "WL";
2823          SlipList.push_back(newSlip);
2824 
2825          // mark it
2826          spdvector[i].flag |= (WLDETECT + WLFIX);
2827 
2828          if(cfg(Debug) >= 7) log << "CHECK " << GDCUnique << " " << sat
2829             << " " << i
2830             << " " << printTime(time(i),outFormat)
2831             << fixed << setprecision(3)
2832             << "  " << pastStats.N()
2833             //<< " " << pastStats.Average()
2834             << " " << pastStats.StdDev()
2835             << "  " << futureStats.N()
2836             //<< " " << futureStats.Average()
2837             << " " << futureStats.StdDev()
2838             << "  " << futureStats.Average() - pastStats.Average()
2839             << " " << nwl
2840             << endl;
2841 
2842       }
2843    }
2844 
2845    return ReturnOK;
2846 }
2847 catch(Exception& e) { GPSTK_RETHROW(e); }
2848 catch(std::exception& e) {
2849    Exception E("std except: "+string(e.what())); GPSTK_THROW(E);
2850 }
2851 catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
2852 }
2853 
2854 //------------------------------------------------------------------------------------
2855 //------------------------------------------------------------------------------------
2856 // last call before returning:
2857 // generate editing commands for deleted (flagged) data,
2858 // use editing command (slips and deletes) to modify the original SatPass data
2859 // print ending summary, and also return it as a string
finish(int iret,SatPass & svp,vector<string> & editCmds)2860 string GDCPass::finish(int iret, SatPass& svp, vector<string>& editCmds)
2861 {
2862 try {
2863    bool ok;
2864    int i,ifirst,ilast,npts;
2865    long N1,N2,prevN1,prevN2;
2866    double slipL1,slipL2,WLbias,GFbias;
2867    //SatPassData spd;
2868    list<Slip>::iterator jt;
2869    string retMessage;
2870 
2871    // ---------------------------------------------------------
2872    // sort the slips in time
2873    SlipList.sort();
2874 
2875    // ---------------------------------------------------------
2876    // merge *this GDCPass and the input SatPass...
2877    // use this->flag to generate edit commands for data marked bad
2878    // use the SlipList to fix slips
2879    // 'change the arrays' A1 and A2 - fill with range minus phase for output
2880    npts = 0;
2881    ilast = -1;                         // ilast is the index of the last good point
2882    ifirst = -1;                        // ifirst is the index of the first good point
2883    WLbias = GFbias = slipL1 = slipL2 = 0.0;
2884    prevN1 = prevN2 = 0L;
2885    jt = SlipList.begin();
2886    for(i=0; i<static_cast<int>(size()); i++) {
2887 
2888       // is this point bad?
2889       if(!(spdvector[i].flag & OK)) {  // data is bad
2890          ok = false;
2891          if(i == static_cast<int>(size()) - 1) {         // but this is the last point
2892             i++;
2893             ok = true;
2894          }
2895       }
2896       else ok = true;                  // data is good
2897 
2898       if(ok) {
2899          if(ifirst == -1) ifirst = i;
2900 
2901          // generate edit commands: delete from ilast+1 to i-1
2902          if(i-ilast > 2 && cfg(OutputDeletes) != 0) {
2903             // delete 2, or a range of, points
2904             // -DS+<sat>,<time>
2905             ostringstream stst1;
2906             stst1 << "-DS";
2907             if(i-ilast > 3) stst1 << "+";
2908             stst1 << sat << ",";
2909             if(cfg(OutputGPSTime))
2910                stst1 << printTime(time(ilast+1),"%F,%.3g");
2911             else
2912                stst1 << printTime(time(ilast+1),"%Y,%m,%d,%H,%M,%f");
2913             if(i-ilast > 3) stst1 << " # begin delete of "
2914                   << asString(i+1-ilast) << " points";
2915             editCmds.push_back(stst1.str());
2916 
2917             // -DS-<sat>,<time>
2918             ostringstream stst2;
2919             stst2 << "-DS";
2920             if(i-ilast > 3) stst2 << "-";
2921             stst2 << sat << ",";
2922             if(cfg(OutputGPSTime))
2923                stst2 << printTime(time(i-1),"%F,%.3g");
2924             else
2925                stst2 << printTime(time(i-1),"%Y,%m,%d,%H,%M,%f");
2926             if(i-ilast > 3) stst2 << " # end delete of "
2927                << asString(i+1-ilast) << " points";
2928             editCmds.push_back(stst2.str());
2929          }
2930          else if(i-ilast > 1 && cfg(OutputDeletes) != 0) {
2931             // delete a single isolated point
2932             ostringstream stst;
2933             stst << "-DS" << sat << ",";
2934             if(cfg(OutputGPSTime))
2935                stst << printTime(time(i-1),"%F,%.3g");
2936             else
2937                stst << printTime(time(i-1),"%Y,%m,%d,%H,%M,%f");
2938             editCmds.push_back(stst.str());
2939          }
2940 
2941          ilast = i;
2942          npts++;
2943       }
2944 
2945       // keep track of net slip fix
2946       if(jt != SlipList.end() && i == jt->index) {          // there is a slip here
2947          // fix the slip by changing the bias added to phase
2948          N1 = jt->N1;
2949          N2 = jt->N1 - jt->NWL;
2950          slipL1 += double(N1);
2951          slipL2 += double(N2);
2952 
2953          // generate edit commands for slips
2954          if(N1-prevN1 != 0) {
2955             ostringstream stst;
2956             stst << "-BD+" << sat << ",L1,";
2957             if(cfg(OutputGPSTime))
2958                stst << printTime(time(jt->index),"%F,%.3g");
2959             else
2960                stst << printTime(time(jt->index),"%Y,%m,%d,%H,%M,%f");
2961             stst << "," << N1-prevN1;
2962             if(!jt->msg.empty()) stst << " # " << jt->msg;
2963             //stst << " # WL: " << jt->NWL << " N1: " << jt->N1; //temp
2964             editCmds.push_back(stst.str());
2965          }
2966          if(N2-prevN2 != 0) {
2967             ostringstream stst;
2968             stst << "-BD+" << sat << ",L2,";
2969             if(cfg(OutputGPSTime))
2970                stst << printTime(time(jt->index),"%F,%.3g");
2971             else
2972                stst << printTime(time(jt->index),"%Y,%m,%d,%H,%M,%f");
2973             stst << "," << N2-prevN2;
2974             if(!jt->msg.empty()) stst << " # " << jt->msg;
2975             editCmds.push_back(stst.str());
2976          }
2977 
2978          prevN1 = N1;
2979          prevN2 = N2;
2980          jt++;
2981       }
2982 
2983       if(i >= static_cast<int>(size())) break;
2984 
2985       // 'change the data' for the last time
2986       spdvector[i].data[L1] = svp.data(i,DCobstypes[L1]) - slipL1;
2987       spdvector[i].data[L2] = svp.data(i,DCobstypes[L2]) - slipL2;
2988       spdvector[i].data[P1] = svp.data(i,DCobstypes[P1]);
2989       spdvector[i].data[P2] = svp.data(i,DCobstypes[P2]);
2990 
2991       // compute range minus phase for output
2992       // do the same at the beginning ("BEG")
2993 
2994       // compute WL and GFP
2995          // narrow lane range (m)
2996       double wlr = wl1r * spdvector[i].data[P1] + wl2r * spdvector[i].data[P2];
2997          // wide lane phase (m)
2998       double wlp = wl1p * spdvector[i].data[L1] + wl2p * spdvector[i].data[L2];
2999          // geo-free range (m)
3000       double gfr = gf1r * spdvector[i].data[P1] + gf2r * spdvector[i].data[P2];
3001          // geo-free phase (m)
3002       double gfp = gf1p * spdvector[i].data[L1] + gf2p * spdvector[i].data[L2];
3003       if(i == ifirst) {
3004          WLbias = (wlp-wlr)/wlwl;
3005          GFbias = gfp;
3006       }
3007       spdvector[i].data[A1] = (wlp-wlr)/wlwl - WLbias; // wide lane bias (cyc)
3008       spdvector[i].data[A2] = gfp - GFbias;            // geo-free phase (m)
3009       //spdvector[i].data[A2] = gfr - gfp;             // geo-free range - phase (m)
3010 
3011    } // end loop over all data
3012 
3013    // first fix the segment for dump - TD? is this necessary?
3014    if(SegList.begin() != SegList.end()) {
3015       SegList.begin()->bias1 = SegList.begin()->bias2 = 0;     // not necessary..
3016       SegList.begin()->nbeg = 0;
3017       SegList.begin()->nend = static_cast<int>(size())-1;
3018       SegList.begin()->npts = npts;
3019    }
3020    // dump the corrected data
3021    if(cfg(Debug) >= 2) dumpSegments("AFT",2,true);
3022 
3023    // dump the edit commands to log
3024    for(i=0; i<static_cast<int>(editCmds.size()); i++)
3025       if(cfg(Debug) >= 2)
3026          log << "EditCmd: " << GDCUnique << " " << editCmds[i] << endl;
3027 
3028    // ---------------------------------------------------------
3029    // copy corrected data into original SatPass, without disturbing other obs types
3030    for(i=0; i<static_cast<int>(size()); i++) {
3031       svp.data(i,DCobstypes[L1]) = spdvector[i].data[L1];
3032       svp.data(i,DCobstypes[L2]) = spdvector[i].data[L2];
3033       svp.data(i,DCobstypes[P1]) = spdvector[i].data[P1];
3034       svp.data(i,DCobstypes[P2]) = spdvector[i].data[P2];
3035 
3036       // change the flag for use by SatPass
3037       //const unsigned short SatPass::OK  = 1; good data
3038       //const unsigned short SatPass::BAD = 0; used by caller and DC to mark bad data
3039       //const unsigned short SatPass::LL1 = 2; discontinuity on L1 only
3040       //const unsigned short SatPass::LL2 = 4; discontinuity on L2 only
3041       //const unsigned short SatPass::LL3 = 6; discontinuity on L1 and L2
3042       //const unsigned short GDCPass::DETECT   =   6;  // = WLDETECT | GFDETECT
3043       //const unsigned short GDCPass::FIX      =  24;  // = WLFIX | GFFIX
3044       if(spdvector[i].flag & OK) {
3045          if(((spdvector[i].flag & DETECT)==0 && (spdvector[i].flag & FIX)!=0)
3046             || i == ifirst)
3047             spdvector[i].flag = LL3 + OK;
3048          else
3049             spdvector[i].flag = OK;
3050       }
3051       else
3052          spdvector[i].flag = BAD;
3053 
3054       svp.LLI(i,DCobstypes[L1]) = (spdvector[i].flag & LL1) ? 1 : 0;
3055       svp.LLI(i,DCobstypes[L2]) = (spdvector[i].flag & LL2) ? 1 : 0;
3056       svp.setFlag(i,spdvector[i].flag);
3057    }
3058 
3059    // ---------------------------------------------------------
3060    // make up string to return
3061    ilast = -1;                               // last good point
3062    ostringstream oss;
3063    for(list<Segment>::iterator it=SegList.begin(); it != SegList.end(); it++) {
3064       i = (it->nend - it->nbeg + 1);         // total number of points
3065       oss << GDCtag << " " << GDCUnique << " " << sat
3066          << " #" << setw(2) << it->nseg << ": "
3067          << setw(4) << it->npts << "/" << setw(4) << i << " pts, # "
3068          << setw(4) << it->nbeg << "-" << setw(4) << it->nend
3069          << " (" << printTime(time(it->nbeg),outFormat)
3070          << " - " << printTime(time(it->nend),outFormat)
3071          << ")";
3072       if(it->npts > 0) {
3073          oss << fixed << setprecision(3)
3074             << " bias(wl)=" << setw(13) << it->bias1 //biaswl
3075             << " bias(gf)=" << setw(13) << it->bias2; //biasgf;
3076          if(ilast > -1) {
3077             ifirst = static_cast<int>(it->nbeg);
3078             while(ifirst <= static_cast<int>(it->nend)
3079                   && !(spdvector[ifirst].flag & OK)) ifirst++;
3080             i = spdvector[ifirst].ndt - spdvector[ilast].ndt;
3081             oss << " gap_segs " << setprecision(1) << setw(5)
3082                << cfg(DT)*i << " s = " << i << " pts.";
3083          }
3084          ilast = static_cast<int>(it->nend);
3085          while(ilast >= static_cast<int>(it->nbeg) && !(spdvector[ilast].flag & OK))
3086             ilast--;
3087       }
3088       oss << endl;
3089    }
3090 
3091    // print the channel number (GLO) and wavelengths in cm
3092    oss << GDCtag << " " << GDCUnique << " " << sat << fixed << setprecision(2)
3093       << " DT " << fixed << setprecision(2) << cfg(DT)
3094       << " wavelengths " << wl1*100.0 << " " << wl2*100.0
3095       << " " << wlwl*100.0 << " " << wlgf*100.0;
3096    if(sat.system == SatelliteSystem::Glonass) oss << " GLOn " << GLOn;
3097    oss << endl;
3098 
3099    // print WL & GF stats for whole pass
3100    if(WLPassStats.N() > 2) {
3101       oss << GDCtag << " " << GDCUnique << " " << sat
3102          << " " << fixed << setprecision(3) << WLPassStats.StdDev()
3103          << " WL sigma in cycles"
3104          << " N=" << WLPassStats.N()
3105          << " Min=" << WLPassStats.Minimum()
3106          << " Max=" << WLPassStats.Maximum()
3107          << " Ave=" << WLPassStats.Average();
3108       if(WLPassStats.StdDev() > cfg(WLSigma))
3109          oss << " Warning - WL sigma > input (" << cfg(WLSigma) << ")";
3110       oss << endl;
3111    }
3112 
3113    if(GFPassStats.N() > 2) {
3114       oss << GDCtag << " " << GDCUnique << " " << sat
3115          << " " << fixed << setprecision(3) << GFPassStats.StdDev()
3116          << " sigma GF variation in meters per DT"
3117          << " N=" << GFPassStats.N()
3118          << " Min=" << GFPassStats.Minimum()
3119          << " Max=" << GFPassStats.Maximum()
3120          << " Ave=" << GFPassStats.Average()
3121          << endl;
3122       oss << GDCtag << " " << GDCUnique << " " << sat
3123          << " " << fixed << setprecision(3)
3124          << (fabs(GFPassStats.Minimum()) > fabs(GFPassStats.Maximum()) ?
3125             fabs(GFPassStats.Minimum()) : fabs(GFPassStats.Maximum()))
3126          << " maximum GF variation in meters per DT"
3127          << " N=" << GFPassStats.N()
3128          << " Ave=" << GFPassStats.Average()
3129          << " Std=" << GFPassStats.StdDev()
3130          << endl;
3131    }
3132 
3133    // print 'learn' summary
3134    map<string,int>::const_iterator kt;
3135    for(kt=learn.begin(); kt != learn.end(); kt++)
3136       oss << GDCtag << " " << GDCUnique << " " << sat
3137          << " " << setw(3) << kt->second << " " << kt->first << endl;
3138    //if(sat.system == SatelliteSystem::Glonass)
3139    //   oss << GDCtag << " " << GDCUnique << " " << sat
3140    //      << "  " << GLOn << string(" GLONASS frequency channel") << endl;
3141 
3142    int n = int((lastTime-firstTime)/cfg(DT)) + 1;
3143    double percent = 100.0*double(ngood)/n;
3144    if(cfg(Debug) > 0) oss << GDCtag << "# " << setw(3) << GDCUnique << ", SAT " << sat
3145       << ", Pts: " << setw(4) << n << " total " << setw(4) << ngood
3146       << " good " << fixed << setprecision(1) << setw(5) << percent << "%"
3147       << ", start " << printTime(firstTime,outFormat)
3148       << endl;
3149 
3150    if(iret) {
3151       oss << GDCtag << " " << setw(3) << GDCUnique << " " << sat
3152          << " " << printTime(firstTime,outFormat)
3153          << " is returning with error code: "
3154          << (iret == NoData ? "insufficient data" :
3155             (iret == BadInput ? "required obs types L1,L2,P1/C1,P2 not found" :
3156             (iret == Singular ? "singularity in polynomial fit" :
3157             (iret == FatalProblem ? "time interval DT was not set" :
3158             (iret == PrematureEnd ? "premature end" : "unknown problem")))))
3159          << endl;
3160    }
3161 
3162    retMessage = oss.str();
3163 
3164    if(cfg(Debug) >= 2) log << "======== End GPSTK Discontinuity Corrector "
3165       << GDCUnique << " ================================================\n";
3166 
3167    stripTrailing(retMessage,'\n');
3168    return retMessage;
3169 }
3170 catch(Exception& e) { GPSTK_RETHROW(e); }
3171 catch(std::exception& e) {
3172    Exception E("std except: "+string(e.what())); GPSTK_THROW(E);
3173 }
3174 catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
3175 }
3176 
3177 //------------------------------------------------------------------------------------
3178 // create, delete and dump Segments
3179 //------------------------------------------------------------------------------------
3180 // create a new segment from the given one, starting at index ibeg,
3181 // and insert it after the given iterator.
3182 // Return an iterator pointing to the new segment. String msg is for debug output
createSegment(list<Segment>::iterator sit,int ibeg,string msg)3183 list<Segment>::iterator GDCPass::createSegment(list<Segment>::iterator sit,
3184                                                int ibeg, string msg)
3185 {
3186 try {
3187    Segment s;
3188    s = *sit;
3189    s.nbeg = ibeg;
3190    s.nend = sit->nend;
3191    sit->nend = ibeg-1;
3192 
3193    // 'trim' beg and end indexes
3194    while(s.nend > s.nbeg && !(spdvector[s.nend].flag & OK)) s.nend--;
3195    while(sit->nend > sit->nbeg && !(spdvector[sit->nend].flag & OK)) sit->nend--;
3196 
3197    // recompute npts // TD is this done somewhere else?
3198    unsigned int i;
3199    s.npts = sit->npts = 0;
3200    for(i=s.nbeg; i<=s.nend; i++)
3201       if(spdvector[i].flag & OK) s.npts++;
3202    for(i=sit->nbeg; i<=sit->nend; i++)
3203       if(spdvector[i].flag & OK) sit->npts++;
3204 
3205    // get the segment number right
3206    s.nseg++;
3207    list<Segment>::iterator skt=sit;
3208    for(skt++; skt != SegList.end(); skt++) skt->nseg++;
3209 
3210    if(cfg(Debug) >= 6)
3211       log << "SEG " << GDCUnique << " " << sat
3212          << " " << msg
3213          << " " << printTime(time(ibeg),outFormat)
3214          << " " << s.nbeg << " - " << s.nend
3215          << " biases " << fixed << setprecision(3) << s.bias1 << " " << s.bias2
3216          << endl;
3217 
3218    learn["breaks found: " + msg]++;
3219 
3220    return SegList.insert(++sit,s); // insert puts s before ++sit
3221 }
3222 catch(Exception& e) { GPSTK_RETHROW(e); }
3223 catch(std::exception& e) {
3224    Exception E("std except: "+string(e.what())); GPSTK_THROW(E);
3225 }
3226 catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
3227 }
3228 
3229 //------------------------------------------------------------------------------------
3230 // dump a list of the segments
3231 // level=0 one line summary (number of segments)
3232 // level=1 one per line list of segments
3233 // level=2 dump all data, including (if extra) temporary arrays
3234 // return level 1 output as string
dumpSegments(string label,int level,bool extra)3235 string GDCPass::dumpSegments(string label, int level, bool extra)
3236 {
3237 try {
3238    unsigned int i,ifirst;
3239    int ilast;
3240    list<Segment>::iterator it;
3241    string msg;
3242    ostringstream oss;
3243 
3244       // summary of SegList
3245    oss << label << " " << GDCUnique
3246       << " list of Segments (" << SegList.size() << "):"
3247       << endl;
3248 
3249    if(level < 1) { msg = oss.str(); log << msg; return msg; }
3250 
3251       // one line per segment
3252    ilast = -1;                               // last good point
3253    for(it=SegList.begin(); it != SegList.end(); it++) {
3254       i = (it->nend - it->nbeg + 1);         // total number of points
3255 
3256       oss << label << " " << GDCUnique << " " << sat
3257          << " #" << setw(2) << it->nseg << ": "
3258          << setw(4) << it->npts << "/" << setw(4) << i << " pts, # "
3259          << setw(4) << it->nbeg << "-" << setw(4) << it->nend
3260          << " (" << printTime(time(it->nbeg),outFormat)
3261          << " - " << printTime(time(it->nend),outFormat)
3262          << ")";
3263 
3264       if(it->npts > 0) {
3265          oss << fixed << setprecision(3)
3266             << " bias(wl)=" << setw(13) << it->bias1 //biaswl
3267             << " bias(gf)=" << setw(13) << it->bias2; //biasgf;
3268          if(ilast > -1) {
3269             ifirst = it->nbeg;
3270             while(ifirst <= it->nend && !(spdvector[ifirst].flag & OK)) ifirst++;
3271             i = spdvector[ifirst].ndt - spdvector[ilast].ndt;
3272             oss << " Gap " << setprecision(1) << setw(5)
3273                << cfg(DT)*i << " s = " << i << " pts.";
3274          }
3275          ilast = it->nend;
3276          while(ilast >= static_cast<int>(it->nbeg) && !(spdvector[ilast].flag & OK))
3277             ilast--;
3278       }
3279 
3280       oss << endl;
3281    }
3282 
3283    if(level < 2) { msg = oss.str(); log << msg; return msg; }
3284 
3285       // dump the data
3286    for(it=SegList.begin(); it != SegList.end(); it++) {
3287       for(i=it->nbeg; i<=it->nend; i++) {
3288 
3289          oss << "DSC" << label << " " << GDCUnique << " " << sat << " " << it->nseg
3290             << " " << printTime(time(i),outFormat)
3291             << " " << setw(3) << spdvector[i].flag
3292             << fixed << setprecision(3)
3293             << " " << setw(13) << spdvector[i].data[L1] - it->bias2 //biasgf  //temp
3294             << " " << setw(13) << spdvector[i].data[L2] - it->bias2 //biasgf
3295             << " " << setw(13) << spdvector[i].data[P1] - it->bias1 //biaswl
3296             << " " << setw(13) << spdvector[i].data[P2];
3297          if(extra) oss
3298             << " " << setw(13) << spdvector[i].data[A1]
3299             << " " << setw(13) << spdvector[i].data[A2];
3300          oss << " " << setw(4) << i;
3301          if(i == it->nbeg) oss
3302             << " " << setw(13) << it->bias1 //biaswl
3303             << " " << setw(13) << it->bias2; //biasgf;
3304          oss << endl;
3305       }
3306    }
3307 
3308    msg = oss.str();
3309    log << msg;
3310    return msg;
3311 }
3312 catch(Exception& e) { GPSTK_RETHROW(e); }
3313 catch(std::exception& e) {
3314    Exception E("std except: "+string(e.what())); GPSTK_THROW(E);
3315 }
3316 catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
3317 }
3318 
3319 //------------------------------------------------------------------------------------
deleteSegment(list<Segment>::iterator & it,string msg)3320 void GDCPass::deleteSegment(list<Segment>::iterator& it, string msg)
3321 {
3322 try {
3323    unsigned int i;
3324 
3325    if(cfg(Debug) >= 6) log << "Delete segment "
3326       << GDCUnique << " " << sat << " " << it->nseg
3327       << " pts " << it->npts
3328       << " indexes " << it->nbeg << " - " << it->nend
3329       << " start " << printTime(firstTime,outFormat)
3330       << " : " << msg
3331       << endl;
3332 
3333    it->npts = 0;
3334    for(i=it->nbeg; i<=it->nend; i++) if(spdvector[i].flag & OK) {
3335       // count these : learn
3336       learn["points deleted: " + msg]++;
3337       spdvector[i].flag = BAD;
3338    }
3339 
3340    learn["segments deleted: " + msg]++;
3341 }
3342 catch(Exception& e) { GPSTK_RETHROW(e); }
3343 catch(std::exception& e) {
3344    Exception E("std except: "+string(e.what())); GPSTK_THROW(E);
3345 }
3346 catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
3347 }
3348 
3349 //------------------------------------------------------------------------------------
3350 //------------------------------------------------------------------------------------
3351