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

Last change on this file since 9270 was 9270, checked in by stuerze, 7 months ago

minor changes

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