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

Last change on this file since 7392 was 7392, checked in by weber, 4 years ago

String waitTime changed to outWait

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