source: ntrip/trunk/BNC/src/combination/bnccomb.cpp @ 9262

Last change on this file since 9262 was 9262, checked in by stuerze, 2 months ago

minor changes

File size: 35.4 KB
Line 
1/* -------------------------------------------------------------------------
2 * BKG NTRIP Client
3 * -------------------------------------------------------------------------
4 *
5 * Class:      bncComb
6 *
7 * Purpose:    Combinations of Orbit/Clock Corrections
8 *
9 * Author:     L. Mervart
10 *
11 * Created:    22-Jan-2011
12 *
13 * Changes:
14 *
15 * -----------------------------------------------------------------------*/
16
17#include <newmatio.h>
18#include <iomanip>
19#include <sstream>
20
21#include "bnccomb.h"
22#include "bnccore.h"
23#include "upload/bncrtnetdecoder.h"
24#include "bncsettings.h"
25#include "bncutils.h"
26#include "bncsp3.h"
27#include "bncantex.h"
28#include "t_prn.h"
29
30const double sig0_offAC    = 1000.0;
31const double sig0_offACSat =  100.0;
32const double sigP_offACSat =   0.01;
33const double sig0_clkSat   =  100.0;
34
35const double sigObs        =   0.05;
36
37using namespace std;
38
39// Constructor
40////////////////////////////////////////////////////////////////////////////
41bncComb::cmbParam::cmbParam(parType type_, int index_, const QString& ac_, const QString& prn_) {
42
43  type   = type_;
44  index  = index_;
45  AC     = ac_;
46  prn    = prn_;
47  xx     = 0.0;
48  eph    = 0;
49
50  if      (type == offACgnss) {
51    epoSpec = true;
52    sig0    = sig0_offAC;
53    sigP    = sig0;
54  }
55  else if (type == offACSat) {
56    epoSpec = false;
57    sig0    = sig0_offACSat;
58    sigP    = sigP_offACSat;
59  }
60  else if (type == clkSat) {
61    epoSpec = true;
62    sig0    = sig0_clkSat;
63    sigP    = sig0;
64  }
65}
66
67// Destructor
68////////////////////////////////////////////////////////////////////////////
69bncComb::cmbParam::~cmbParam() {
70}
71
72// Partial
73////////////////////////////////////////////////////////////////////////////
74double bncComb::cmbParam::partial(char sys, const QString& AC_, const QString& prn_) {
75
76  if      (type == offACgnss) {
77    if (AC == AC_ && prn_[0].toLatin1() == sys) {
78      return 1.0;
79    }
80  }
81  else if (type == offACSat) {
82    if (AC == AC_ && prn == prn_) {
83      return 1.0;
84    }
85  }
86  else if (type == clkSat) {
87    if (prn == prn_) {
88      return 1.0;
89    }
90  }
91
92  return 0.0;
93}
94
95//
96////////////////////////////////////////////////////////////////////////////
97QString bncComb::cmbParam::toString(char sys) const {
98
99  QString outStr;
100
101  if      (type == offACgnss) {
102    outStr = "AC  Offset " + AC + " " + sys + "  ";
103  }
104  else if (type == offACSat) {
105    outStr = "Sat Offset " + AC + " " + prn.mid(0,3);
106  }
107  else if (type == clkSat) {
108    outStr = "Clk Corr " + prn.mid(0,3);
109  }
110
111  return outStr;
112}
113
114// Singleton
115////////////////////////////////////////////////////////////////////////////
116bncComb* bncComb::instance() {
117  static bncComb _bncComb;
118  return &_bncComb;
119}
120
121// Constructor
122////////////////////////////////////////////////////////////////////////////
123bncComb::bncComb() : _ephUser(true) {
124
125  bncSettings settings;
126
127  QStringList cmbStreams = settings.value("cmbStreams").toStringList();
128
129  _cmbSampl = settings.value("cmbSampl").toInt();
130  if (_cmbSampl <= 0) {
131    _cmbSampl = 10;
132  }
133
134  _cmbSysPrn['G'] = t_prn::MAXPRN_GPS;     _masterMissingEpochs['G'] = 0;
135  _cmbSysPrn['R'] = t_prn::MAXPRN_GLONASS; _masterMissingEpochs['R'] = 0;
136  _cmbSysPrn['E'] = t_prn::MAXPRN_GALILEO; _masterMissingEpochs['E'] = 0;
137  _cmbSysPrn['C'] = t_prn::MAXPRN_BDS;     _masterMissingEpochs['C'] = 0;/*
138  _cmbSysPrn['J'] = t_prn::MAXPRN_QZSS;    _masterMissingEpochs['J'] = 0;
139  _cmbSysPrn['S'] = t_prn::MAXPRN_SBAS;    _masterMissingEpochs['S'] = 0;
140  */
141
142  if (cmbStreams.size() >= 1 && !cmbStreams[0].isEmpty()) {
143    QListIterator<QString> it(cmbStreams);
144    while (it.hasNext()) {
145      QStringList hlp = it.next().split(" ");
146      cmbAC* newAC = new cmbAC();
147      newAC->mountPoint = hlp[0];
148      newAC->name       = hlp[1];
149      newAC->weight     = hlp[2].toDouble();
150      QMapIterator<char, unsigned> itSys(_cmbSysPrn);
151      // init
152      while (itSys.hasNext()) {
153        itSys.next();
154        char sys = itSys.key();
155        if (_masterOrbitAC.isEmpty()) {
156          _masterOrbitAC[sys] = newAC->name;
157        }
158      }
159      _ACs.append(newAC);
160    }
161  }
162
163  QString ssrFormat;
164  _ssrCorr = 0;
165  QListIterator<QString> it(settings.value("uploadMountpointsOut").toStringList());
166  while (it.hasNext()) {
167    QStringList hlp = it.next().split(",");
168    if (hlp.size() > 7) {
169      ssrFormat = hlp[7];
170    }
171  }
172  if      (ssrFormat == "IGS-SSR") {
173    _ssrCorr = new SsrCorrIgs();
174  }
175  else if (ssrFormat == "RTCM-SSR") {
176    _ssrCorr = new SsrCorrRtcm();
177  }
178  else { // default
179    _ssrCorr = new SsrCorrIgs();
180  }
181
182  _rtnetDecoder = 0;
183
184  connect(this,     SIGNAL(newMessage(QByteArray,bool)),
185          BNC_CORE, SLOT(slotMessage(const QByteArray,bool)));
186
187  connect(BNC_CORE, SIGNAL(providerIDChanged(QString)),
188          this,     SLOT(slotProviderIDChanged(QString)));
189
190  connect(BNC_CORE, SIGNAL(newOrbCorrections(QList<t_orbCorr>)),
191          this,     SLOT(slotNewOrbCorrections(QList<t_orbCorr>)));
192
193  connect(BNC_CORE, SIGNAL(newClkCorrections(QList<t_clkCorr>)),
194          this,     SLOT(slotNewClkCorrections(QList<t_clkCorr>)));
195
196
197
198  // Combination Method
199  // ------------------
200  if (settings.value("cmbMethod").toString() == "Single-Epoch") {
201    _method = singleEpoch;
202  }
203  else {
204    _method = filter;
205  }
206
207  // Initialize Parameters (model: Clk_Corr = AC_Offset + Sat_Offset + Clk)
208  // ----------------------------------------------------------------------
209  if (_method == filter) {
210    // SYSTEM
211    QMapIterator<char, unsigned> itSys(_cmbSysPrn);
212    while (itSys.hasNext()) {
213      itSys.next();
214      int nextPar = 0;
215      char sys = itSys.key();
216      unsigned maxPrn = itSys.value();
217      unsigned flag = 0;
218      if (sys == 'E') {
219        flag = 1;
220      }
221      // AC
222      QListIterator<cmbAC*> itAc(_ACs);
223      while (itAc.hasNext()) {
224        cmbAC* AC = itAc.next();
225        _params[sys].push_back(new cmbParam(cmbParam::offACgnss, ++nextPar, AC->name, ""));
226        for (unsigned iGnss = 1; iGnss <= maxPrn; iGnss++) {
227          QString prn = QString("%1%2_%3").arg(sys).arg(iGnss, 2, 10, QChar('0')).arg(flag);
228          _params[sys].push_back(new cmbParam(cmbParam::offACSat, ++nextPar, AC->name, prn));
229        }
230      }
231      for (unsigned iGnss = 1; iGnss <= maxPrn; iGnss++) {
232        QString prn = QString("%1%2_%3").arg(sys).arg(iGnss, 2, 10, QChar('0')).arg(flag);
233        _params[sys].push_back(new cmbParam(cmbParam::clkSat, ++nextPar, "", prn));
234      }
235      // Initialize Variance-Covariance Matrix
236      // -------------------------------------
237      _QQ[sys].ReSize(_params[sys].size());
238      _QQ[sys] = 0.0;
239      for (int iPar = 1; iPar <= _params[sys].size(); iPar++) {
240        cmbParam* pp = _params[sys][iPar-1];
241        _QQ[sys](iPar,iPar) = pp->sig0 * pp->sig0;
242      }
243    }
244  }
245
246  // ANTEX File
247  // ----------
248  _antex = 0;
249  QString antexFileName = settings.value("uploadAntexFile").toString();
250  if (!antexFileName.isEmpty()) {
251    _antex = new bncAntex();
252    if (_antex->readFile(antexFileName) != success) {
253      emit newMessage("wrong ANTEX file", true);
254      delete _antex;
255      _antex = 0;
256    }
257  }
258
259  // Maximal Residuum
260  // ----------------
261  _MAXRES = settings.value("cmbMaxres").toDouble();
262  if (_MAXRES <= 0.0) {
263    _MAXRES = 999.0;
264  }
265}
266
267// Destructor
268////////////////////////////////////////////////////////////////////////////
269bncComb::~bncComb() {
270  QListIterator<cmbAC*> icAC(_ACs);
271  while (icAC.hasNext()) {
272    delete icAC.next();
273  }
274  delete _rtnetDecoder;
275  if (_ssrCorr) {
276    delete _ssrCorr;
277  }
278  delete _antex;
279  QMapIterator<char, unsigned> itSys(_cmbSysPrn);
280  while (itSys.hasNext()) {
281    itSys.next();
282    char sys = itSys.key();
283    for (int iPar = 1; iPar <= _params[sys].size(); iPar++) {
284      delete _params[sys][iPar-1];
285    }
286    QListIterator<bncTime> itTime(_buffer[sys].keys());
287    while (itTime.hasNext()) {
288      bncTime epoTime = itTime.next();
289      _buffer[sys].remove(epoTime);
290    }
291  }
292
293}
294
295// Remember orbit corrections
296////////////////////////////////////////////////////////////////////////////
297void bncComb::slotNewOrbCorrections(QList<t_orbCorr> orbCorrections) {
298  QMutexLocker locker(&_mutex);
299
300  for (int ii = 0; ii < orbCorrections.size(); ii++) {
301    t_orbCorr& orbCorr = orbCorrections[ii];
302    QString    staID(orbCorr._staID.c_str());
303    QString    prn(orbCorr._prn.toInternalString().c_str());
304
305    // Find/Check the AC Name
306    // ----------------------
307    QString acName;
308    QListIterator<cmbAC*> icAC(_ACs);
309    while (icAC.hasNext()) {
310      cmbAC* AC = icAC.next();
311      if (AC->mountPoint == staID) {
312        acName = AC->name;
313        break;
314      }
315    }
316    if (acName.isEmpty()) {
317      continue;
318    }
319
320    // Store the correction
321    // --------------------
322    QMap<t_prn, t_orbCorr>& storage = _orbCorrections[acName];
323    storage[orbCorr._prn] = orbCorr;
324  }
325}
326
327// Process clock corrections
328////////////////////////////////////////////////////////////////////////////
329void bncComb::slotNewClkCorrections(QList<t_clkCorr> clkCorrections) {
330  QMutexLocker locker(&_mutex);
331
332  bncTime lastTime;
333
334  for (int ii = 0; ii < clkCorrections.size(); ii++) {
335    t_clkCorr& clkCorr = clkCorrections[ii];
336    QString    staID(clkCorr._staID.c_str());
337    QString    prn(clkCorr._prn.toInternalString().c_str());
338    char       sys = clkCorr._prn.system();
339
340    // Set the last time
341    // -----------------
342    if (lastTime.undef() || clkCorr._time > lastTime) {
343      lastTime = clkCorr._time;
344    }
345
346    // Find/Check the AC Name
347    // ----------------------
348    QString acName;
349    QListIterator<cmbAC*> icAC(_ACs);
350    while (icAC.hasNext()) {
351      cmbAC* AC = icAC.next();
352      if (AC->mountPoint == staID) {
353        acName = AC->name;
354        break;
355      }
356    }
357    if (acName.isEmpty()) {
358      continue;
359    }
360
361    // Check Modulo Time
362    // -----------------
363    int sec = int(nint(clkCorr._time.gpssec()*10));
364    if (sec % (_cmbSampl*10) != 0.0) {
365      continue;
366    }
367
368    // Check Correction Age
369    // --------------------
370    if (_resTime.valid() && clkCorr._time <= _resTime) {
371      emit newMessage("bncComb: old correction: " + acName.toLatin1() + " " + prn.mid(0,3).toLatin1(), true);
372      continue;
373    }
374
375    // Create new correction
376    // ---------------------
377    cmbCorr* newCorr  = new cmbCorr();
378    newCorr->_prn     = prn;
379    newCorr->_time    = clkCorr._time;
380    newCorr->_iod     = clkCorr._iod;
381    newCorr->_acName  = acName;
382    newCorr->_clkCorr = clkCorr;
383
384    // Check orbit correction
385    // ----------------------
386    if (!_orbCorrections.contains(acName)) {
387      delete newCorr;
388      continue;
389    }
390    else {
391      QMap<t_prn, t_orbCorr>& storage = _orbCorrections[acName];
392      if (!storage.contains(clkCorr._prn)  ||
393           storage[clkCorr._prn]._iod != newCorr->_iod) {
394        delete newCorr;
395        continue;
396      }
397      else {
398        newCorr->_orbCorr = storage[clkCorr._prn];
399      }
400    }
401
402    // Check the Ephemeris
403    //--------------------
404    t_eph* ephLast = _ephUser.ephLast(prn);
405    t_eph* ephPrev = _ephUser.ephPrev(prn);
406    if (ephLast == 0) {
407      //emit newMessage("bncComb: eph not found for "  + prn.mid(0,3).toLatin1(), true);
408      delete newCorr;
409      continue;
410    }
411    else {
412      if      (ephLast->IOD() == newCorr->_iod) {
413        newCorr->_eph = ephLast;
414      }
415      else if (ephPrev && ephPrev->IOD() == newCorr->_iod) {
416        newCorr->_eph = ephPrev;
417        switchToLastEph(ephLast, newCorr);
418      }
419      else {
420        //emit newMessage("bncComb: eph not found for "  + prn.mid(0,3).toLatin1() +
421                        //QString(" with IOD %1").arg(newCorr->_iod).toLatin1(), true);
422        delete newCorr;
423        continue;
424      }
425    }
426
427    // Store correction into the buffer
428    // --------------------------------
429    QVector<cmbCorr*>& corrs = _buffer[sys][newCorr->_time].corrs;
430    corrs.push_back(newCorr);
431
432
433  }
434
435  // Process previous Epoch(s)
436  // -------------------------
437  const double outWait = 1.0 * _cmbSampl;
438  QMapIterator<char, unsigned> itSys(_cmbSysPrn);
439  while (itSys.hasNext()) {
440    itSys.next();
441    char sys = itSys.key();
442    QListIterator<bncTime> itTime(_buffer[sys].keys());
443    while (itTime.hasNext()) {
444      bncTime epoTime = itTime.next();
445      if (epoTime < lastTime - outWait) {
446        _resTime = epoTime;
447        processEpoch(sys);
448      }
449    }
450  }
451}
452
453// Change the correction so that it refers to last received ephemeris
454////////////////////////////////////////////////////////////////////////////
455void bncComb::switchToLastEph(t_eph* lastEph, cmbCorr* corr) {
456
457  if (corr->_eph == lastEph) {
458    return;
459  }
460
461  ColumnVector oldXC(6);
462  ColumnVector oldVV(3);
463  corr->_eph->getCrd(corr->_time, oldXC, oldVV, false);
464
465  ColumnVector newXC(6);
466  ColumnVector newVV(3);
467  lastEph->getCrd(corr->_time, newXC, newVV, false);
468
469  ColumnVector dX = newXC.Rows(1,3) - oldXC.Rows(1,3);
470  ColumnVector dV = newVV           - oldVV;
471  double       dC = newXC(4)        - oldXC(4);
472
473  // small check
474  // -----------
475  const double MAXDIFF = 1000.0;
476  double diff  = dX.NormFrobenius();
477  double diffC = dC * t_CST::c ;
478  if (diff  > MAXDIFF ||
479      diffC > MAXDIFF) {
480    return;
481  }
482
483  ColumnVector dRAO(3);
484  XYZ_to_RSW(newXC.Rows(1,3), newVV, dX, dRAO);
485
486  ColumnVector dDotRAO(3);
487  XYZ_to_RSW(newXC.Rows(1,3), newVV, dV, dDotRAO);
488
489  QString msg = "switch corr " + corr->_prn.mid(0,3)
490    + QString(" %1 -> %2 %3").arg(corr->_iod,3).arg(lastEph->IOD(),3).arg(dC*t_CST::c, 8, 'f', 4);
491
492  emit newMessage(msg.toLatin1(), false);
493
494  corr->_iod = lastEph->IOD();
495  corr->_eph = lastEph;
496
497  corr->_orbCorr._xr    += dRAO;
498  corr->_orbCorr._dotXr += dDotRAO;
499  corr->_clkCorr._dClk  -= dC;
500}
501
502// Process Epoch
503////////////////////////////////////////////////////////////////////////////
504void bncComb::processEpoch(char sys) {
505
506  _log.clear();
507
508  QTextStream out(&_log, QIODevice::WriteOnly);
509
510  out << endl <<           "Combination: " << sys << endl
511      << "--------------------------------" << endl;
512
513  // Observation Statistics
514  // ----------------------
515  bool masterPresent = false;
516  QListIterator<cmbAC*> icAC(_ACs);
517  while (icAC.hasNext()) {
518    cmbAC* AC = icAC.next();
519    AC->numObs[sys] = 0;
520    QVectorIterator<cmbCorr*> itCorr(corrs(sys));
521    while (itCorr.hasNext()) {
522      cmbCorr* corr = itCorr.next();
523      if (corr->_acName == AC->name) {
524        AC->numObs[sys] += 1;
525        if (AC->name == _masterOrbitAC[sys]) {
526          masterPresent = true;
527        }
528      }
529    }
530    out << AC->name.toLatin1().data() << ": " << AC->numObs[sys] << endl;
531  }
532
533  // If Master not present, switch to another one
534  // --------------------------------------------
535  const unsigned switchMasterAfterGap = 1;
536  if (masterPresent) {
537    _masterMissingEpochs[sys] = 0;
538  }
539  else {
540    ++_masterMissingEpochs[sys];
541    if (_masterMissingEpochs[sys] < switchMasterAfterGap) {
542      out << "Missing Master, Epoch skipped" << endl;
543      _buffer[sys].remove(_resTime);
544      emit newMessage(_log, false);
545      return;
546    }
547    else {
548      _masterMissingEpochs[sys] = 0;
549      QListIterator<cmbAC*> icAC(_ACs);
550      while (icAC.hasNext()) {
551        cmbAC* AC = icAC.next();
552        if (AC->numObs[sys] > 0) {
553          out << "Switching Master AC "
554              << _masterOrbitAC[sys].toLatin1().data() << " --> "
555              << AC->name.toLatin1().data()   << " "
556              << _resTime.datestr().c_str()    << " "
557              << _resTime.timestr().c_str()    << endl;
558          _masterOrbitAC[sys] = AC->name;
559          break;
560        }
561      }
562    }
563  }
564
565  QMap<QString, cmbCorr*> resCorr;
566
567  // Perform the actual Combination using selected Method
568  // ----------------------------------------------------
569  t_irc irc;
570  ColumnVector dx;
571  if (_method == filter) {
572    irc = processEpoch_filter(sys, out, resCorr, dx);
573  }
574  else {
575    irc = processEpoch_singleEpoch(sys, out, resCorr, dx);
576  }
577
578  // Update Parameter Values, Print Results
579  // --------------------------------------
580  if (irc == success) {
581    for (int iPar = 1; iPar <= _params[sys].size(); iPar++) {
582      cmbParam* pp = _params[sys][iPar-1];
583      pp->xx += dx(iPar);
584      if (pp->type == cmbParam::clkSat) {
585        if (resCorr.find(pp->prn) != resCorr.end()) {
586          resCorr[pp->prn]->_dClkResult = pp->xx / t_CST::c;
587        }
588      }
589      out << _resTime.datestr().c_str() << " "
590          << _resTime.timestr().c_str() << " ";
591      out.setRealNumberNotation(QTextStream::FixedNotation);
592      out.setFieldWidth(8);
593      out.setRealNumberPrecision(4);
594      out << pp->toString(sys) << " "
595          << pp->xx << " +- " << sqrt(_QQ[sys](pp->index,pp->index)) << endl;
596      out.setFieldWidth(0);
597    }
598    printResults(out, resCorr);
599    dumpResults(resCorr);
600  }
601
602  // Delete Data, emit Message
603  // -------------------------
604  _buffer[sys].remove(_resTime);
605  emit newMessage(_log, false);
606}
607
608// Process Epoch - Filter Method
609////////////////////////////////////////////////////////////////////////////
610t_irc bncComb::processEpoch_filter(char sys, QTextStream& out,
611                                   QMap<QString, cmbCorr*>& resCorr,
612                                   ColumnVector& dx) {
613
614  // Prediction Step
615  // ---------------
616  int nPar = _params[sys].size();
617  ColumnVector x0(nPar);
618  for (int iPar = 1; iPar <= nPar; iPar++) {
619    cmbParam* pp  = _params[sys][iPar-1];
620    if (pp->epoSpec) {
621      pp->xx = 0.0;
622      _QQ[sys].Row(iPar)    = 0.0;
623      _QQ[sys].Column(iPar) = 0.0;
624      _QQ[sys](iPar,iPar) = pp->sig0 * pp->sig0;
625    }
626    else {
627      _QQ[sys](iPar,iPar) += pp->sigP * pp->sigP;
628    }
629    x0(iPar) = pp->xx;
630  }
631
632  // Check Satellite Positions for Outliers
633  // --------------------------------------
634  if (checkOrbits(sys, out) != success) {
635    return failure;
636  }
637
638  // Update and outlier detection loop
639  // ---------------------------------
640  SymmetricMatrix QQ_sav = _QQ[sys];
641  while (true) {
642
643    Matrix         AA;
644    ColumnVector   ll;
645    DiagonalMatrix PP;
646
647    if (createAmat(sys, AA, ll, PP, x0, resCorr) != success) {
648      return failure;
649    }
650
651    dx.ReSize(nPar); dx = 0.0;
652    kalman(AA, ll, PP, _QQ[sys], dx);
653
654    ColumnVector vv = ll - AA * dx;
655
656    int     maxResIndex;
657    double  maxRes = vv.maximum_absolute_value1(maxResIndex);
658    out.setRealNumberNotation(QTextStream::FixedNotation);
659    out.setRealNumberPrecision(3);
660    out << _resTime.datestr().c_str() << " " << _resTime.timestr().c_str()
661        << " Maximum Residuum " << maxRes << ' '
662        << corrs(sys)[maxResIndex-1]->_acName << ' ' << corrs(sys)[maxResIndex-1]->_prn.mid(0,3);
663    if (maxRes > _MAXRES) {
664      for (int iPar = 1; iPar <= _params[sys].size(); iPar++) {
665        cmbParam* pp = _params[sys][iPar-1];
666        if (pp->type == cmbParam::offACSat            &&
667            pp->AC   == corrs(sys)[maxResIndex-1]->_acName &&
668            pp->prn  == corrs(sys)[maxResIndex-1]->_prn.mid(0,3)) {
669          QQ_sav.Row(iPar)    = 0.0;
670          QQ_sav.Column(iPar) = 0.0;
671          QQ_sav(iPar,iPar)   = pp->sig0 * pp->sig0;
672        }
673      }
674
675      out << "  Outlier" << endl;
676      _QQ[sys] = QQ_sav;
677      corrs(sys).remove(maxResIndex-1);
678    }
679    else {
680      out << "  OK" << endl;
681      out.setRealNumberNotation(QTextStream::FixedNotation);
682      out.setRealNumberPrecision(4);
683      for (int ii = 0; ii < corrs(sys).size(); ii++) {
684      const cmbCorr* corr = corrs(sys)[ii];
685        out << _resTime.datestr().c_str() << ' '
686            << _resTime.timestr().c_str() << " "
687            << corr->_acName << ' ' << corr->_prn.mid(0,3);
688        out.setFieldWidth(10);
689        out <<  " res = " << vv[ii] << endl;
690        out.setFieldWidth(0);
691      }
692      break;
693    }
694  }
695
696  return success;
697}
698
699// Print results
700////////////////////////////////////////////////////////////////////////////
701void bncComb::printResults(QTextStream& out,
702                           const QMap<QString, cmbCorr*>& resCorr) {
703
704  QMapIterator<QString, cmbCorr*> it(resCorr);
705  while (it.hasNext()) {
706    it.next();
707    cmbCorr* corr = it.value();
708    const t_eph* eph = corr->_eph;
709    if (eph) {
710      ColumnVector xc(6);
711      ColumnVector vv(3);
712      eph->getCrd(_resTime, xc, vv, false);
713
714      out << _resTime.datestr().c_str() << " "
715          << _resTime.timestr().c_str() << " ";
716      out.setFieldWidth(3);
717      out << "Full Clock " << corr->_prn.mid(0,3) << " " << corr->_iod << " ";
718      out.setFieldWidth(14);
719      out << (xc(4) + corr->_dClkResult) * t_CST::c << endl;
720      out.setFieldWidth(0);
721    }
722    else {
723      out << "bncComb::printResuls bug" << endl;
724    }
725  }
726}
727
728// Send results to RTNet Decoder and directly to PPP Client
729////////////////////////////////////////////////////////////////////////////
730void bncComb::dumpResults(const QMap<QString, cmbCorr*>& resCorr) {
731
732  QList<t_orbCorr> orbCorrections;
733  QList<t_clkCorr> clkCorrections;
734
735  QString     outLines;
736
737  unsigned year, month, day, hour, minute;
738  double   sec;
739  _resTime.civil_date(year, month, day);
740  _resTime.civil_time(hour, minute, sec);
741
742  outLines.sprintf("*  %4d %2d %2d %d %d %12.8f\n",
743                   year, month, day, hour, minute, sec);
744
745  QMapIterator<QString, cmbCorr*> it(resCorr);
746  while (it.hasNext()) {
747    it.next();
748    cmbCorr* corr = it.value();
749
750    t_orbCorr orbCorr(corr->_orbCorr);
751    orbCorr._staID = "INTERNAL";
752    orbCorrections.push_back(orbCorr);
753
754    t_clkCorr clkCorr(corr->_clkCorr);
755    clkCorr._staID      = "INTERNAL";
756    clkCorr._dClk       = corr->_dClkResult;
757    clkCorr._dotDClk    = 0.0;
758    clkCorr._dotDotDClk = 0.0;
759    clkCorrections.push_back(clkCorr);
760
761    ColumnVector xc(6);
762    ColumnVector vv(3);
763    corr->_eph->setClkCorr(dynamic_cast<const t_clkCorr*>(&clkCorr));
764    corr->_eph->setOrbCorr(dynamic_cast<const t_orbCorr*>(&orbCorr));
765    corr->_eph->getCrd(_resTime, xc, vv, true);
766
767    // Correction Phase Center --> CoM
768    // -------------------------------
769    ColumnVector dx(3); dx = 0.0;
770    if (_antex) {
771      double Mjd = _resTime.mjd() + _resTime.daysec()/86400.0;
772      if (_antex->satCoMcorrection(corr->_prn, Mjd, xc.Rows(1,3), dx) != success) {
773        dx = 0;
774        _log += "antenna not found " + corr->_prn.mid(0,3).toLatin1() + '\n';
775      }
776    }
777
778    outLines += corr->_prn.mid(0,3);
779    QString hlp;
780    hlp.sprintf(" APC 3 %15.4f %15.4f %15.4f"
781                " Clk 1 %15.4f"
782                " Vel 3 %15.4f %15.4f %15.4f"
783                " CoM 3 %15.4f %15.4f %15.4f\n",
784                xc(1), xc(2), xc(3),
785                xc(4) *  t_CST::c,
786                vv(1), vv(2), vv(3),
787                xc(1)-dx(1), xc(2)-dx(2), xc(3)-dx(3));
788    outLines += hlp;
789
790    QString line;
791    int messageType   = _ssrCorr->COTYPE_GPSCOMBINED;
792    int updateInt     = 0;
793    line.sprintf("%d %d %d %.1f %s"
794                 "   %lu"
795                 "   %8.3f %8.3f %8.3f %8.3f"
796                 "   %10.5f %10.5f %10.5f %10.5f"
797                 "   %10.5f INTERNAL",
798                 messageType, updateInt, _resTime.gpsw(), _resTime.gpssec(),
799                 corr->_prn.mid(0,3).toLatin1().data(),
800                 corr->_iod,
801                 corr->_dClkResult * t_CST::c,
802                 corr->_orbCorr._xr[0],
803                 corr->_orbCorr._xr[1],
804                 corr->_orbCorr._xr[2],
805                 0.0,
806                 corr->_orbCorr._dotXr[0],
807                 corr->_orbCorr._dotXr[1],
808                 corr->_orbCorr._dotXr[2],
809                 0.0);
810
811    delete corr;
812  }
813
814  outLines += "EOE\n"; // End Of Epoch flag
815
816  if (!_rtnetDecoder) {
817    _rtnetDecoder = new bncRtnetDecoder();
818  }
819
820  vector<string> errmsg;
821  _rtnetDecoder->Decode(outLines.toLatin1().data(), outLines.length(), errmsg);
822
823  // Send new Corrections to PPP etc.
824  // --------------------------------
825  if (orbCorrections.size() > 0 && clkCorrections.size() > 0) {
826    emit newOrbCorrections(orbCorrections);
827    emit newClkCorrections(clkCorrections);
828  }
829}
830
831// Create First Design Matrix and Vector of Measurements
832////////////////////////////////////////////////////////////////////////////
833t_irc bncComb::createAmat(char sys, Matrix& AA, ColumnVector& ll, DiagonalMatrix& PP,
834                          const ColumnVector& x0, QMap<QString, cmbCorr*>& resCorr) {
835
836  unsigned nPar = _params[sys].size();
837  unsigned nObs = corrs(sys).size();
838
839  if (nObs == 0) {
840    return failure;
841  }
842
843  int maxSat = _cmbSysPrn[sys];
844
845  const int nCon = (_method == filter) ? 1 + maxSat : 0;
846
847  AA.ReSize(nObs+nCon, nPar);  AA = 0.0;
848  ll.ReSize(nObs+nCon);        ll = 0.0;
849  PP.ReSize(nObs+nCon);        PP = 1.0 / (sigObs * sigObs);
850
851  int iObs = 0;
852  QVectorIterator<cmbCorr*> itCorr(corrs(sys));
853  while (itCorr.hasNext()) {
854    cmbCorr* corr = itCorr.next();
855    QString  prn  = corr->_prn;
856
857    ++iObs;
858
859    if (corr->_acName == _masterOrbitAC[sys] && resCorr.find(prn) == resCorr.end()) {
860      resCorr[prn] = new cmbCorr(*corr);
861    }
862
863    for (int iPar = 1; iPar <= _params[sys].size(); iPar++) {
864      cmbParam* pp = _params[sys][iPar-1];
865      AA(iObs, iPar) = pp->partial(sys, corr->_acName, prn);
866    }
867
868    ll(iObs) = corr->_clkCorr._dClk * t_CST::c - DotProduct(AA.Row(iObs), x0);
869  }
870
871  // Regularization
872  // --------------
873  if (_method == filter) {
874    const double Ph = 1.e6;
875    PP(nObs+1) = Ph;
876    for (int iPar = 1; iPar <= _params[sys].size(); iPar++) {
877      cmbParam* pp = _params[sys][iPar-1];
878      if ( AA.Column(iPar).maximum_absolute_value() > 0.0 &&
879           pp->type == cmbParam::clkSat ) {
880        AA(nObs+1, iPar) = 1.0;
881      }
882    }
883    unsigned flag = 0;
884    if (sys == 'E') {
885      flag = 1;
886    }
887    if (sys == 'R') {
888      return success;
889    }
890    int iCond = 1;
891    // GNSS
892    for (unsigned iGnss = 1; iGnss <= _cmbSysPrn[sys]; iGnss++) {
893      QString prn = QString("%1%2_%3").arg(sys).arg(iGnss, 2, 10, QChar('0')).arg(flag);
894      ++iCond;
895      PP(nObs+iCond) = Ph;
896      for (int iPar = 1; iPar <= _params[sys].size(); iPar++) {
897        cmbParam* pp = _params[sys][iPar-1];
898        if ( pp &&
899             AA.Column(iPar).maximum_absolute_value() > 0.0 &&
900             pp->type == cmbParam::offACSat                 &&
901             pp->prn == prn) {
902          AA(nObs+iCond, iPar) = 1.0;
903        }
904      }
905    }
906  }
907
908  return success;
909}
910
911// Process Epoch - Single-Epoch Method
912////////////////////////////////////////////////////////////////////////////
913t_irc bncComb::processEpoch_singleEpoch(char sys, QTextStream& out,
914                                        QMap<QString, cmbCorr*>& resCorr,
915                                        ColumnVector& dx) {
916
917  // Check Satellite Positions for Outliers
918  // --------------------------------------
919  if (checkOrbits(sys, out) != success) {
920    return failure;
921  }
922
923  // Outlier Detection Loop
924  // ----------------------
925  while (true) {
926
927    // Remove Satellites that are not in Master
928    // ----------------------------------------
929    QMutableVectorIterator<cmbCorr*> it(corrs(sys));
930    while (it.hasNext()) {
931      cmbCorr* corr = it.next();
932      QString  prn  = corr->_prn;
933      bool foundMaster = false;
934      QVectorIterator<cmbCorr*> itHlp(corrs(sys));
935      while (itHlp.hasNext()) {
936        cmbCorr* corrHlp = itHlp.next();
937        QString  prnHlp  = corrHlp->_prn;
938        QString  ACHlp   = corrHlp->_acName;
939        if (ACHlp == _masterOrbitAC[sys] && prn == prnHlp) {
940          foundMaster = true;
941          break;
942        }
943      }
944      if (!foundMaster) {
945        delete corr;
946        it.remove();
947      }
948    }
949
950    // Count Number of Observations per Satellite and per AC
951    // -----------------------------------------------------
952    QMap<QString, int> numObsPrn;
953    QMap<QString, int> numObsAC;
954    QVectorIterator<cmbCorr*> itCorr(corrs(sys));
955    while (itCorr.hasNext()) {
956      cmbCorr* corr = itCorr.next();
957      QString  prn  = corr->_prn;
958      QString  AC   = corr->_acName;
959      if (numObsPrn.find(prn) == numObsPrn.end()) {
960        numObsPrn[prn]  = 1;
961      }
962      else {
963        numObsPrn[prn] += 1;
964      }
965      if (numObsAC.find(AC) == numObsAC.end()) {
966        numObsAC[AC]  = 1;
967      }
968      else {
969        numObsAC[AC] += 1;
970      }
971    }
972
973    // Clean-Up the Parameters
974    // -----------------------
975    for (int iPar = 1; iPar <= _params[sys].size(); iPar++) {
976      delete _params[sys][iPar-1];
977    }
978    _params[sys].clear();
979
980    // Set new Parameters
981    // ------------------
982    int nextPar = 0;
983
984    QMapIterator<QString, int> itAC(numObsAC);
985    while (itAC.hasNext()) {
986      itAC.next();
987      const QString& AC     = itAC.key();
988      int            numObs = itAC.value();
989      if (AC != _masterOrbitAC[sys] && numObs > 0) {
990        _params[sys].push_back(new cmbParam(cmbParam::offACgnss, ++nextPar, AC, ""));
991      }
992    }
993
994    QMapIterator<QString, int> itPrn(numObsPrn);
995    while (itPrn.hasNext()) {
996      itPrn.next();
997      const QString& prn    = itPrn.key();
998      int            numObs = itPrn.value();
999      if (numObs > 0) {
1000        _params[sys].push_back(new cmbParam(cmbParam::clkSat, ++nextPar, "", prn));
1001      }
1002    }
1003
1004    int nPar = _params[sys].size();
1005    ColumnVector x0(nPar);
1006    x0 = 0.0;
1007
1008    // Create First-Design Matrix
1009    // --------------------------
1010    Matrix         AA;
1011    ColumnVector   ll;
1012    DiagonalMatrix PP;
1013    if (createAmat(sys, AA, ll, PP, x0, resCorr) != success) {
1014      return failure;
1015    }
1016
1017    ColumnVector vv;
1018    try {
1019      Matrix          ATP = AA.t() * PP;
1020      SymmetricMatrix NN; NN << ATP * AA;
1021      ColumnVector    bb = ATP * ll;
1022      _QQ[sys] = NN.i();
1023      dx  = _QQ[sys] * bb;
1024      vv  = ll - AA * dx;
1025    }
1026    catch (Exception& exc) {
1027      out << exc.what() << endl;
1028      return failure;
1029    }
1030
1031    int     maxResIndex;
1032    double  maxRes = vv.maximum_absolute_value1(maxResIndex);
1033    out.setRealNumberNotation(QTextStream::FixedNotation);
1034    out.setRealNumberPrecision(3);
1035    out << _resTime.datestr().c_str() << " " << _resTime.timestr().c_str()
1036        << " Maximum Residuum " << maxRes << ' '
1037        << corrs(sys)[maxResIndex-1]->_acName << ' ' << corrs(sys)[maxResIndex-1]->_prn.mid(0,3);
1038
1039    if (maxRes > _MAXRES) {
1040      out << "  Outlier" << endl;
1041      delete corrs(sys)[maxResIndex-1];
1042      corrs(sys).remove(maxResIndex-1);
1043    }
1044    else {
1045      out << "  OK" << endl;
1046      out.setRealNumberNotation(QTextStream::FixedNotation);
1047      out.setRealNumberPrecision(3);
1048      for (int ii = 0; ii < vv.Nrows(); ii++) {
1049        const cmbCorr* corr = corrs(sys)[ii];
1050        out << _resTime.datestr().c_str() << ' '
1051            << _resTime.timestr().c_str() << " "
1052            << corr->_acName << ' ' << corr->_prn.mid(0,3);
1053        out.setFieldWidth(6);
1054        out << " res = " << vv[ii] << endl;
1055        out.setFieldWidth(0);
1056      }
1057      return success;
1058    }
1059
1060  }
1061
1062  return failure;
1063}
1064
1065// Check Satellite Positions for Outliers
1066////////////////////////////////////////////////////////////////////////////
1067t_irc bncComb::checkOrbits(char sys, QTextStream& out) {
1068
1069  const double MAX_DISPLACEMENT = 0.20;
1070
1071  // Switch to last ephemeris (if possible)
1072  // --------------------------------------
1073  QMutableVectorIterator<cmbCorr*> im(corrs(sys));
1074  while (im.hasNext()) {
1075    cmbCorr* corr = im.next();
1076    QString  prn  = corr->_prn;
1077
1078    t_eph* ephLast = _ephUser.ephLast(prn);
1079    t_eph* ephPrev = _ephUser.ephPrev(prn);
1080
1081    if      (ephLast == 0) {
1082      out << "checkOrbit: missing eph (not found) " << corr->_prn.mid(0,3) << endl;
1083      delete corr;
1084      im.remove();
1085    }
1086    else if (corr->_eph == 0) {
1087      out << "checkOrbit: missing eph (zero) " << corr->_prn.mid(0,3) << endl;
1088      delete corr;
1089      im.remove();
1090    }
1091    else {
1092      if ( corr->_eph == ephLast || corr->_eph == ephPrev ) {
1093        switchToLastEph(ephLast, corr);
1094      }
1095      else {
1096        out << "checkOrbit: missing eph (deleted) " << corr->_prn.mid(0,3) << endl;
1097        delete corr;
1098        im.remove();
1099      }
1100    }
1101  }
1102
1103  while (true) {
1104
1105    // Compute Mean Corrections for all Satellites
1106    // -------------------------------------------
1107    QMap<QString, int>          numCorr;
1108    QMap<QString, ColumnVector> meanRao;
1109    QVectorIterator<cmbCorr*> it(corrs(sys));
1110    while (it.hasNext()) {
1111      cmbCorr* corr = it.next();
1112      QString  prn  = corr->_prn;
1113      if (meanRao.find(prn) == meanRao.end()) {
1114        meanRao[prn].ReSize(4);
1115        meanRao[prn].Rows(1,3) = corr->_orbCorr._xr;
1116        meanRao[prn](4)        = 1;
1117      }
1118      else {
1119        meanRao[prn].Rows(1,3) += corr->_orbCorr._xr;
1120        meanRao[prn](4)        += 1;
1121      }
1122      if (numCorr.find(prn) == numCorr.end()) {
1123        numCorr[prn] = 1;
1124      }
1125      else {
1126        numCorr[prn] += 1;
1127      }
1128    }
1129
1130    // Compute Differences wrt. Mean, find Maximum
1131    // -------------------------------------------
1132    QMap<QString, cmbCorr*> maxDiff;
1133    it.toFront();
1134    while (it.hasNext()) {
1135      cmbCorr* corr = it.next();
1136      QString  prn  = corr->_prn;
1137      if (meanRao[prn](4) != 0) {
1138        meanRao[prn] /= meanRao[prn](4);
1139        meanRao[prn](4) = 0;
1140      }
1141      corr->_diffRao = corr->_orbCorr._xr - meanRao[prn].Rows(1,3);
1142      if (maxDiff.find(prn) == maxDiff.end()) {
1143        maxDiff[prn] = corr;
1144      }
1145      else {
1146        double normMax = maxDiff[prn]->_diffRao.NormFrobenius();
1147        double norm    = corr->_diffRao.NormFrobenius();
1148        if (norm > normMax) {
1149          maxDiff[prn] = corr;
1150        }
1151      }
1152    }
1153
1154    if (_ACs.size() == 1) {
1155      break;
1156    }
1157
1158    // Remove Outliers
1159    // ---------------
1160    bool removed = false;
1161    QMutableVectorIterator<cmbCorr*> im(corrs(sys));
1162    while (im.hasNext()) {
1163      cmbCorr* corr = im.next();
1164      QString  prn  = corr->_prn;
1165      if      (numCorr[prn] < 2) {
1166        delete corr;
1167        im.remove();
1168      }
1169      else if (corr == maxDiff[prn]) {
1170        double norm = corr->_diffRao.NormFrobenius();
1171        if (norm > MAX_DISPLACEMENT) {
1172          out << _resTime.datestr().c_str()    << " "
1173              << _resTime.timestr().c_str()    << " "
1174              << "Orbit Outlier: "
1175              << corr->_acName.toLatin1().data() << " "
1176              << prn.mid(0,3).toLatin1().data()           << " "
1177              << corr->_iod                     << " "
1178              << norm                           << endl;
1179          delete corr;
1180          im.remove();
1181          removed = true;
1182        }
1183      }
1184    }
1185
1186    if (!removed) {
1187      break;
1188    }
1189  }
1190
1191  return success;
1192}
1193
1194//
1195////////////////////////////////////////////////////////////////////////////
1196void bncComb::slotProviderIDChanged(QString mountPoint) {
1197  QMutexLocker locker(&_mutex);
1198
1199  QTextStream out(&_log, QIODevice::WriteOnly);
1200
1201  // Find the AC Name
1202  // ----------------
1203  QString acName;
1204  QListIterator<cmbAC*> icAC(_ACs);
1205  while (icAC.hasNext()) {
1206    cmbAC* AC = icAC.next();
1207    if (AC->mountPoint == mountPoint) {
1208      acName = AC->name;
1209      out << "Provider ID changed: AC " << AC->name.toLatin1().data()   << " "
1210          << _resTime.datestr().c_str()    << " "
1211          << _resTime.timestr().c_str()    << endl;
1212      break;
1213    }
1214  }
1215  if (acName.isEmpty()) {
1216    return;
1217  }
1218  QMapIterator<char, unsigned> itSys(_cmbSysPrn);
1219  while (itSys.hasNext()) {
1220    itSys.next();
1221    char sys = itSys.key();
1222    // Remove all corrections of the corresponding AC
1223    // ----------------------------------------------
1224    QListIterator<bncTime> itTime(_buffer[sys].keys());
1225    while (itTime.hasNext()) {
1226      bncTime epoTime = itTime.next();
1227      QVector<cmbCorr*>& corrVec = _buffer[sys][epoTime].corrs;
1228      QMutableVectorIterator<cmbCorr*> it(corrVec);
1229      while (it.hasNext()) {
1230        cmbCorr* corr = it.next();
1231        if (acName == corr->_acName) {
1232          delete corr;
1233          it.remove();
1234        }
1235      }
1236    }
1237
1238    // Reset Satellite Offsets
1239    // -----------------------
1240    if (_method == filter) {
1241      for (int iPar = 1; iPar <= _params[sys].size(); iPar++) {
1242        cmbParam* pp = _params[sys][iPar-1];
1243        if (pp->AC == acName && pp->type == cmbParam::offACSat) {
1244          pp->xx = 0.0;
1245          _QQ[sys].Row(iPar)    = 0.0;
1246          _QQ[sys].Column(iPar) = 0.0;
1247          _QQ[sys](iPar,iPar) = pp->sig0 * pp->sig0;
1248        }
1249      }
1250    }
1251  }
1252}
Note: See TracBrowser for help on using the repository browser.