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

Last change on this file since 9530 was 9530, checked in by stuerze, 8 months ago

minor changes

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