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