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

Last change on this file since 8483 was 8483, checked in by stuerze, 11 months ago

SSR parameter clock rate, clock drift and URA are added within RTNET format

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