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

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

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

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