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

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

minor changes

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