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

Last change on this file since 9025 was 9025, checked in by stuerze, 2 years ago

some modification to allow encoding and decoding of SSR corrections in RTCM-SSR and IGS-SSR formats

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