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

Last change on this file since 9268 was 9268, checked in by stuerze, 4 years ago

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

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