source: ntrip/branches/BNC_2.12/src/combination/bnccomb.cpp @ 9269

Last change on this file since 9269 was 9269, checked in by stuerze, 5 months ago

minor changes to prevent erroneous epehemeris data sets from usage in combination

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