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

Last change on this file since 10371 was 10330, checked in by stuerze, 10 months ago

another test is added in PPP and combination mode to check if stored ephemerides were outdated and/or not updated in between
File size: 46.4 KB
Line 
1/* -------------------------------------------------------------------------
2 * BKG NTRIP Client
3 * -------------------------------------------------------------------------
4 *
5 * Class: bncComb
6 *
7 * Purpose: Combinations of Orbit/Clock Corrections
8 *
9 * Author: L. Mervart
10 *
11 * Created: 22-Jan-2011
12 *
13 * Changes:
14 *
15 * -----------------------------------------------------------------------*/
16
17#include <newmatio.h>
18#include <iomanip>
19#include <sstream>
20#include <map>
21
22#include "bnccomb.h"
23#include <combination/bncbiassnx.h>
24#include "bnccore.h"
25#include "upload/bncrtnetdecoder.h"
26#include "bncsettings.h"
27#include "bncutils.h"
28#include "bncantex.h"
29#include "t_prn.h"
30
31const double sig0_offAC = 1000.0;
32const double sig0_offACSat = 100.0;
33const double sigP_offACSat = 0.01;
34const double sig0_clkSat = 100.0;
35
36const double sigObs = 0.05;
37
38using namespace std;
39
40// Constructor
41////////////////////////////////////////////////////////////////////////////
42bncComb::cmbParam::cmbParam(parType type_, int index_, const QString& ac_, const QString& prn_) {
43
44 type = type_;
45 index = index_;
46 AC = ac_;
47 prn = prn_;
48 xx = 0.0;
49 eph = 0;
50
51 if (type == offACgnss) {
52 epoSpec = true;
53 sig0 = sig0_offAC;
54 sigP = sig0;
55 }
56 else if (type == offACSat) {
57 epoSpec = false;
58 sig0 = sig0_offACSat;
59 sigP = sigP_offACSat;
60 }
61 else if (type == clkSat) {
62 epoSpec = true;
63 sig0 = sig0_clkSat;
64 sigP = sig0;
65 }
66}
67
68// Destructor
69////////////////////////////////////////////////////////////////////////////
70bncComb::cmbParam::~cmbParam() {
71}
72
73// Partial
74////////////////////////////////////////////////////////////////////////////
75double bncComb::cmbParam::partial(char sys, const QString& AC_, const QString& prn_) {
76
77 if (type == offACgnss) {
78 if (AC == AC_ && prn_[0].toLatin1() == sys) {
79 return 1.0;
80 }
81 }
82 else if (type == offACSat) {
83 if (AC == AC_ && prn == prn_) {
84 return 1.0;
85 }
86 }
87 else if (type == clkSat) {
88 if (prn == prn_) {
89 return 1.0;
90 }
91 }
92
93 return 0.0;
94}
95
96//
97////////////////////////////////////////////////////////////////////////////
98QString bncComb::cmbParam::toString(char sys) const {
99
100 QString outStr;
101
102 if (type == offACgnss) {
103 outStr = "AC Offset " + AC + " " + sys + " ";
104 }
105 else if (type == offACSat) {
106 outStr = "Sat Offset " + AC + " " + prn.mid(0,3);
107 }
108 else if (type == clkSat) {
109 outStr = "Clk Corr " + prn.mid(0,3);
110 }
111
112 return outStr;
113}
114
115// Singleton: definition class variable
116////////////////////////////////////////////////////////////////////////////
117bncComb* bncComb::instance = nullptr;
118
119
120// Constructor
121////////////////////////////////////////////////////////////////////////////
122bncComb::bncComb() : _ephUser(true) {
123
124 bncSettings settings;
125
126 _running = true;
127
128 QStringList cmbStreams = settings.value("cmbStreams").toStringList();
129
130 _cmbSampl = settings.value("cmbSampl").toInt();
131 if (_cmbSampl <= 0) {
132 _cmbSampl = 5;
133 }
134 _useGps = (Qt::CheckState(settings.value("cmbGps").toInt()) == Qt::Checked) ? true : false;
135 if (_useGps) {
136 _cmbSysPrn['G'] = t_prn::MAXPRN_GPS;
137 _masterMissingEpochs['G'] = 0;
138 }
139 _useGlo = (Qt::CheckState(settings.value("cmbGlo").toInt()) == Qt::Checked) ? true : false;
140 if (_useGlo) {
141 _cmbSysPrn['R'] = t_prn::MAXPRN_GLONASS;
142 _masterMissingEpochs['R'] = 0;
143 }
144 _useGal = (Qt::CheckState(settings.value("cmbGal").toInt()) == Qt::Checked) ? true : false;
145 if (_useGal) {
146 _cmbSysPrn['E'] = t_prn::MAXPRN_GALILEO;
147 _masterMissingEpochs['E'] = 0;
148 }
149 _useBds = (Qt::CheckState(settings.value("cmbBds").toInt()) == Qt::Checked) ? true : false;
150 if (_useBds) {
151 _cmbSysPrn['C'] = t_prn::MAXPRN_BDS;
152 _masterMissingEpochs['C'] = 0;
153 }
154 _useQzss = (Qt::CheckState(settings.value("cmbQzss").toInt()) == Qt::Checked) ? true : false;
155 if (_useQzss) {
156 _cmbSysPrn['J'] = t_prn::MAXPRN_QZSS;
157 _masterMissingEpochs['J'] = 0;
158 }
159 _useSbas = (Qt::CheckState(settings.value("cmbSbas").toInt()) == Qt::Checked) ? true : false;
160 if (_useSbas) {
161 _cmbSysPrn['S'] = t_prn::MAXPRN_SBAS;
162 _masterMissingEpochs['S'] = 0;
163 }
164 _useIrnss = (Qt::CheckState(settings.value("cmbIrnss").toInt()) == Qt::Checked) ? true : false;
165 if (_useIrnss) {
166 _cmbSysPrn['I'] = t_prn::MAXPRN_IRNSS;
167 _masterMissingEpochs['I'] = 0;
168 }
169
170 if (cmbStreams.size() >= 1 && !cmbStreams[0].isEmpty()) {
171 QListIterator<QString> it(cmbStreams);
172 while (it.hasNext()) {
173 QStringList hlp = it.next().split(" ");
174 cmbAC* newAC = new cmbAC();
175 newAC->mountPoint = hlp[0];
176 newAC->name = hlp[1];
177 newAC->weightFactor = hlp[2].toDouble();
178 newAC->isAPC = bool(newAC->mountPoint.mid(0,4) == "SSRA");
179 QMapIterator<char, unsigned> itSys(_cmbSysPrn);
180 // init
181 while (itSys.hasNext()) {
182 itSys.next();
183 char sys = itSys.key();
184 if (!_masterOrbitAC.contains(sys)) {
185 _masterOrbitAC[sys] = newAC->name;
186 _masterIsAPC[sys] = newAC->isAPC;
187 }
188 }
189 _ACs.append(newAC);
190 }
191 }
192
193 QString ssrFormat;
194 _ssrCorr = 0;
195 QListIterator<QString> it(settings.value("uploadMountpointsOut").toStringList());
196 while (it.hasNext()) {
197 QStringList hlp = it.next().split(",");
198 if (hlp.size() > 7) {
199 ssrFormat = hlp[7];
200 }
201 }
202 if (ssrFormat == "IGS-SSR") {
203 _ssrCorr = new SsrCorrIgs();
204 }
205 else if (ssrFormat == "RTCM-SSR") {
206 _ssrCorr = new SsrCorrRtcm();
207 }
208 else { // default
209 _ssrCorr = new SsrCorrIgs();
210 }
211
212 _rtnetDecoder = new bncRtnetDecoder();
213
214 connect(this, SIGNAL(newMessage(QByteArray,bool)), BNC_CORE, SLOT(slotMessage(const QByteArray,bool)));
215 connect(BNC_CORE, SIGNAL(providerIDChanged(QString)), this, SLOT(slotProviderIDChanged(QString)));
216 connect(BNC_CORE, SIGNAL(newOrbCorrections(QList<t_orbCorr>)), this, SLOT(slotNewOrbCorrections(QList<t_orbCorr>)));
217 connect(BNC_CORE, SIGNAL(newClkCorrections(QList<t_clkCorr>)), this, SLOT(slotNewClkCorrections(QList<t_clkCorr>)));
218 connect(BNC_CORE, SIGNAL(newCodeBiases(QList<t_satCodeBias>)), this, SLOT(slotNewCodeBiases(QList<t_satCodeBias>)));
219
220 // Combination Method
221 // ------------------
222 if (settings.value("cmbMethod").toString() == "Single-Epoch") {
223 _method = singleEpoch;
224 }
225 else {
226 _method = filter;
227 }
228
229 // Initialize Parameters (model: Clk_Corr = AC_Offset + Sat_Offset + Clk)
230 // ----------------------------------------------------------------------
231 if (_method == filter) {
232 // SYSTEM
233 QMapIterator<char, unsigned> itSys(_cmbSysPrn);
234 while (itSys.hasNext()) {
235 itSys.next();
236 int nextPar = 0;
237 char sys = itSys.key();
238 unsigned maxPrn = itSys.value();
239 unsigned flag = 0;
240 if (sys == 'E') {
241 flag = 1;
242 }
243 // AC
244 QListIterator<cmbAC*> itAc(_ACs);
245 while (itAc.hasNext()) {
246 cmbAC* AC = itAc.next();
247 _params[sys].push_back(new cmbParam(cmbParam::offACgnss, ++nextPar, AC->name, ""));
248 for (unsigned iGnss = 1; iGnss <= maxPrn; iGnss++) {
249 QString prn = QString("%1%2_%3").arg(sys).arg(iGnss, 2, 10, QChar('0')).arg(flag);
250 _params[sys].push_back(new cmbParam(cmbParam::offACSat, ++nextPar, AC->name, prn));
251 }
252 }
253 for (unsigned iGnss = 1; iGnss <= maxPrn; iGnss++) {
254 QString prn = QString("%1%2_%3").arg(sys).arg(iGnss, 2, 10, QChar('0')).arg(flag);
255 _params[sys].push_back(new cmbParam(cmbParam::clkSat, ++nextPar, "", prn));
256 }
257 // Initialize Variance-Covariance Matrix
258 // -------------------------------------
259 _QQ[sys].ReSize(_params[sys].size());
260 _QQ[sys] = 0.0;
261 for (int iPar = 1; iPar <= _params[sys].size(); iPar++) {
262 cmbParam* pp = _params[sys][iPar-1];
263 _QQ[sys](iPar,iPar) = pp->sig0 * pp->sig0;
264 }
265 }
266 }
267
268 // ANTEX File
269 // ----------
270 _antex = 0;
271 QString antexFileName = settings.value("uploadAntexFile").toString();
272 if (!antexFileName.isEmpty()) {
273 _antex = new bncAntex();
274 if (_antex->readFile(antexFileName) != success) {
275 emit newMessage("bncCmb: wrong ANTEX file", true);
276 delete _antex;
277 _antex = 0;
278 }
279 }
280
281
282 // Bias SINEX File
283 // ---------------
284 _bsx = 0;
285 QString bsxFileName = settings.value("cmbBsxFile").toString();
286 if (!bsxFileName.isEmpty()) {
287 _bsx = new bncBiasSnx();
288 if (_bsx->readFile(bsxFileName) != success) {
289 emit newMessage("bncComb: wrong Bias SINEX file", true);
290 delete _bsx;
291 _bsx = 0;
292 }
293 }
294 if (_bsx) {
295 _ms = 3.0 * 3600 * 1000.0;
296 QTimer::singleShot(_ms, this, SLOT(slotReadBiasSnxFile()));
297 }
298
299 // Maximal Residuum
300 // ----------------
301 _MAXRES = settings.value("cmbMaxres").toDouble();
302 if (_MAXRES <= 0.0) {
303 _MAXRES = 999.0;
304 }
305 _newCorr = 0;
306}
307
308// Destructor
309////////////////////////////////////////////////////////////////////////////
310bncComb::~bncComb() {
311
312 _running = false;
313#ifndef WIN32
314 sleep(2);
315#else
316 Sleep(2000);
317#endif
318
319 QListIterator<cmbAC*> icAC(_ACs);
320 while (icAC.hasNext()) {
321 delete icAC.next();
322 }
323 _ACs.clear();
324
325 delete _rtnetDecoder;
326
327 if (_ssrCorr) {
328 delete _ssrCorr;
329 }
330
331 if (_newCorr) {
332 delete _newCorr;
333 }
334
335 delete _antex;
336 delete _bsx;
337
338 QMapIterator<char, unsigned> itSys(_cmbSysPrn);
339 while (itSys.hasNext()) {
340 itSys.next();
341 char sys = itSys.key();
342 for (int iPar = 1; iPar <= _params[sys].size(); iPar++) {
343 delete _params[sys][iPar-1];
344 }
345 _buffer.remove(sys);
346 }
347 _params.clear();
348 _buffer.clear();
349 _cmbSysPrn.clear();
350
351 while (!_epoClkData.empty()) {
352 delete _epoClkData.front();
353 _epoClkData.pop_front();
354 }
355}
356
357void bncComb::slotReadBiasSnxFile() {
358 bncSettings settings;
359 QString bsxFileName = settings.value("cmbBsxFile").toString();
360
361 if (bsxFileName.isEmpty()) {
362 emit newMessage("bncComb: no Bias SINEX file specified", true);
363 return;
364 }
365 if (!_bsx) {
366 _bsx = new bncBiasSnx();
367 }
368 if (_bsx->readFile(bsxFileName) != success) {
369 emit newMessage("bncComb: wrong Bias SINEX file", true);
370 delete _bsx;
371 _bsx = 0;
372 }
373 else {
374 emit newMessage("bncComb: successfully read Bias SINEX file", true);
375 }
376
377 QTimer::singleShot(_ms, this, SLOT(slotReadBiasSnxFile()));
378}
379
380
381// Remember orbit corrections
382////////////////////////////////////////////////////////////////////////////
383void bncComb::slotNewOrbCorrections(QList<t_orbCorr> orbCorrections) {
384 QMutexLocker locker(&_mutex);
385 for (int ii = 0; ii < orbCorrections.size(); ii++) {
386 t_orbCorr& orbCorr = orbCorrections[ii];
387 QString staID(orbCorr._staID.c_str());
388 char sys = orbCorr._prn.system();
389
390 if (!_cmbSysPrn.contains(sys)){
391 continue;
392 }
393
394 // Find/Check the AC Name
395 // ----------------------
396 QString acName;
397 QListIterator<cmbAC*> icAC(_ACs);
398 while (icAC.hasNext()) {
399 cmbAC* AC = icAC.next();
400 if (AC->mountPoint == staID) {
401 acName = AC->name;
402 break;
403 }
404 }
405 if (acName.isEmpty()) {
406 continue;
407 }
408
409 // Store the correction
410 // --------------------
411 QMap<t_prn, t_orbCorr>& storage = _orbCorrections[acName];
412 storage[orbCorr._prn] = orbCorr;
413 }
414}
415
416// Remember Satellite Code Biases
417////////////////////////////////////////////////////////////////////////////
418void bncComb::slotNewCodeBiases(QList<t_satCodeBias> satCodeBiases) {
419 QMutexLocker locker(&_mutex);
420
421 for (int ii = 0; ii < satCodeBiases.size(); ii++) {
422 t_satCodeBias& satCodeBias = satCodeBiases[ii];
423 QString staID(satCodeBias._staID.c_str());
424 char sys = satCodeBias._prn.system();
425
426 if (!_cmbSysPrn.contains(sys)){
427 continue;
428 }
429
430 // Find/Check the AC Name
431 // ----------------------
432 QString acName;
433 QListIterator<cmbAC*> icAC(_ACs);
434 while (icAC.hasNext()) {
435 cmbAC* AC = icAC.next();
436 if (AC->mountPoint == staID) {
437 acName = AC->name;
438 break;
439 }
440 }
441 if (acName.isEmpty()) {
442 continue;
443 }
444
445 // Store the correction
446 // --------------------
447 QMap<t_prn, t_satCodeBias>& storage = _satCodeBiases[acName];
448 storage[satCodeBias._prn] = satCodeBias;
449 }
450
451}
452
453// Process clock corrections
454////////////////////////////////////////////////////////////////////////////
455void bncComb::slotNewClkCorrections(QList<t_clkCorr> clkCorrections) {
456 QMutexLocker locker(&_mutex);
457 const double outWait = 1.0 * _cmbSampl;
458 int currentWeek = 0;
459 double currentSec = 0.0;
460 currentGPSWeeks(currentWeek, currentSec);
461 bncTime currentTime(currentWeek, currentSec);
462
463 // Loop over all observations (possible different epochs)
464 // -----------------------------------------------------
465 for (int ii = 0; ii < clkCorrections.size(); ii++) {
466 t_clkCorr& newClk = clkCorrections[ii];
467 char sys = newClk._prn.system();
468 if (!_cmbSysPrn.contains(sys)){
469 continue;
470 }
471
472 // Check Modulo Time
473 // -----------------
474 int sec = int(nint(newClk._time.gpssec()*10));
475 if (sec % (_cmbSampl*10) != 0.0) {
476 continue;
477 }
478
479 // Find/Check the AC Name
480 // ----------------------
481 QString acName;
482 bool isAPC;
483 QString staID(newClk._staID.c_str());
484 QListIterator<cmbAC*> icAC(_ACs);
485 while (icAC.hasNext()) {
486 cmbAC* AC = icAC.next();
487 if (AC->mountPoint == staID) {
488 acName = AC->name;
489 isAPC = AC->isAPC;
490 break;
491 }
492 }
493 if (acName.isEmpty() || isAPC != _masterIsAPC[sys]) {
494 continue;
495 }
496
497 // Check regarding current time
498 // ----------------------------
499 if (BNC_CORE->mode() != t_bncCore::batchPostProcessing) {
500 if ((newClk._time >= currentTime) || // future time stamp
501 (currentTime - newClk._time) > 60.0) { // very old data sets
502 #ifdef BNC_DEBUG_CMB
503 emit newMessage("bncComb: future or very old data sets: " + acName.toLatin1() + " " + newClk._prn.toString().c_str() +
504 QString(" %1 ").arg(newClk._time.timestr().c_str()).toLatin1() + "vs. current time" +
505 QString(" %1 ").arg(currentTime.timestr().c_str()).toLatin1(), true);
506 #endif
507 continue;
508 }
509 }
510
511 // Check Correction Age
512 // --------------------
513 if (_resTime.valid() && newClk._time <= _resTime) {
514#ifdef BNC_DEBUG_CMB
515 emit newMessage("bncComb: old correction: " + acName.toLatin1() + " " + newClk._prn.toString().c_str() +
516 QString(" %1 ").arg(newClk._time.timestr().c_str()).toLatin1() + "vs. last processing Epoch" +
517 QString(" %1 ").arg(_resTime.timestr().c_str()).toLatin1(), true);
518#endif
519 continue;
520 }
521
522 // Set the last time
523 // -----------------
524 if (_lastClkCorrTime.undef() || newClk._time > _lastClkCorrTime) {
525 _lastClkCorrTime = newClk._time;
526 }
527
528 // Find the corresponding data epoch or create a new one
529 // -----------------------------------------------------
530 epoClkData* epoch = 0;
531 deque<epoClkData*>::const_iterator it;
532 for (it = _epoClkData.begin(); it != _epoClkData.end(); it++) {
533 if (newClk._time == (*it)->_time) {
534 epoch = *it;
535 break;
536 }
537 }
538 if (epoch == 0) {
539 if (_epoClkData.empty() || newClk._time > _epoClkData.back()->_time) {
540 epoch = new epoClkData;
541 epoch->_time = newClk._time;
542 _epoClkData.push_back(epoch);
543 }
544 }
545
546 // Put data into the epoch
547 // -----------------------
548 if (epoch != 0) {
549 epoch->_clkCorr.push_back(newClk);
550 }
551 }
552
553 // Make sure the buffer does not grow beyond any limit
554 // ---------------------------------------------------
555 const unsigned MAX_EPODATA_SIZE = 60.0 / _cmbSampl;
556 if (_epoClkData.size() > MAX_EPODATA_SIZE) {
557 delete _epoClkData.front();
558 _epoClkData.pop_front();
559 }
560
561 // Process the oldest epochs
562 // ------------------------
563 while (_epoClkData.size()) {
564
565 if (!_lastClkCorrTime.valid()) {
566 return;
567 }
568
569 const vector<t_clkCorr>& clkCorrVec = _epoClkData.front()->_clkCorr;
570
571 bncTime epoTime = _epoClkData.front()->_time;
572
573 if (BNC_CORE->mode() != t_bncCore::batchPostProcessing) {
574 if ((currentTime - epoTime) > 60.0) {
575 delete _epoClkData.front();
576 _epoClkData.pop_front();
577 continue;
578 }
579 }
580 // Process the front epoch
581 // -----------------------
582 if (epoTime < (_lastClkCorrTime - outWait)) {
583 _resTime = epoTime;
584 processEpoch(_resTime, clkCorrVec);
585 delete _epoClkData.front();
586 _epoClkData.pop_front();
587 }
588 else {
589 return;
590 }
591 }
592}
593
594// Change the correction so that it refers to last received ephemeris
595////////////////////////////////////////////////////////////////////////////
596void bncComb::switchToLastEph(t_eph* lastEph, cmbCorr* corr) {
597
598 if (corr->_eph == lastEph) {
599 return;
600 }
601
602 ColumnVector oldXC(6);
603 ColumnVector oldVV(3);
604 if (corr->_eph->getCrd(corr->_time, oldXC, oldVV, false) != success) {
605 return;
606 }
607
608 ColumnVector newXC(6);
609 ColumnVector newVV(3);
610 if (lastEph->getCrd(corr->_time, newXC, newVV, false) != success) {
611 return;
612 }
613
614 ColumnVector dX = newXC.Rows(1,3) - oldXC.Rows(1,3);
615 ColumnVector dV = newVV - oldVV;
616 double dC = newXC(4) - oldXC(4);
617
618 ColumnVector dRAO(3);
619 XYZ_to_RSW(newXC.Rows(1,3), newVV, dX, dRAO);
620
621 ColumnVector dDotRAO(3);
622 XYZ_to_RSW(newXC.Rows(1,3), newVV, dV, dDotRAO);
623
624 QString msg = "switch corr " + corr->_prn.mid(0,3)
625 + QString(" %1 -> %2 %3").arg(corr->_iod,3).arg(lastEph->IOD(),3).arg(dC*t_CST::c, 8, 'f', 4);
626
627 emit newMessage(msg.toLatin1(), false);
628
629 corr->_iod = lastEph->IOD();
630 corr->_eph = lastEph;
631
632 corr->_orbCorr._xr += dRAO;
633 corr->_orbCorr._dotXr += dDotRAO;
634 corr->_clkCorr._dClk -= dC;
635}
636
637// Process Epoch
638////////////////////////////////////////////////////////////////////////////
639void bncComb::processEpoch(bncTime epoTime, const vector<t_clkCorr>& clkCorrVec) {
640 QDateTime now = currentDateAndTimeGPS();
641 bncTime currentTime(now.toString(Qt::ISODate).toStdString());
642 for (unsigned ii = 0; ii < clkCorrVec.size(); ii++) {
643 const t_clkCorr& clkCorr = clkCorrVec[ii];
644 QString staID(clkCorr._staID.c_str());
645 QString prn(clkCorr._prn.toInternalString().c_str());
646 char sys = clkCorr._prn.system();
647
648 // Find/Check the AC Name
649 // ----------------------
650 QString acName;
651 double weigthFactor;
652 QListIterator<cmbAC*> icAC(_ACs);
653 while (icAC.hasNext()) {
654 cmbAC* AC = icAC.next();
655 if (AC->mountPoint == staID) {
656 acName = AC->name;
657 weigthFactor = AC->weightFactor;
658 break;
659 }
660 }
661
662 // Create new correction
663 // ---------------------
664 _newCorr = new cmbCorr();
665 _newCorr->_prn = prn;
666 _newCorr->_time = clkCorr._time;
667 _newCorr->_iod = clkCorr._iod;
668 _newCorr->_acName = acName;
669 _newCorr->_weightFactor = weigthFactor;
670 _newCorr->_clkCorr = clkCorr;
671
672 // Check orbit correction
673 // ----------------------
674 if (!_orbCorrections.contains(acName)) {
675 delete _newCorr; _newCorr = 0;
676 continue;
677 }
678 else {
679 QMap<t_prn, t_orbCorr>& storage = _orbCorrections[acName];
680 if (!storage.contains(clkCorr._prn) ||
681 storage[clkCorr._prn]._iod != _newCorr->_iod) {
682 delete _newCorr; _newCorr = 0;
683 continue;
684 }
685 else {
686 _newCorr->_orbCorr = storage[clkCorr._prn];
687 }
688 }
689
690 // Check the Ephemeris
691 //--------------------
692 t_eph* ephLast = _ephUser.ephLast(prn);
693 t_eph* ephPrev = _ephUser.ephPrev(prn);
694 if (ephLast == 0) {
695#ifdef BNC_DEBUG_CMB
696 emit newMessage("bncComb: eph not found for " + prn.mid(0,3).toLatin1(), true);
697#endif
698 delete _newCorr; _newCorr = 0;
699 continue;
700 }
701 else if (outDatedBcep(ephLast, currentTime) ||
702 ephLast->checkState() == t_eph::bad ||
703 ephLast->checkState() == t_eph::outdated ||
704 ephLast->checkState() == t_eph::unhealthy) {
705#ifdef BNC_DEBUG_CMB
706 emit newMessage("bncComb: ephLast not ok (checkState: " + ephLast->checkStateToString().toLatin1() + ") for " + prn.mid(0,3).toLatin1(), true);
707#endif
708 delete _newCorr; _newCorr = 0;
709 continue;
710 }
711 else {
712 if (ephLast->IOD() == _newCorr->_iod) {
713 _newCorr->_eph = ephLast;
714 }
715 else if (ephPrev &&
716 !outDatedBcep(ephPrev, currentTime) && // if not updated in storage
717 ephPrev->checkState() == t_eph::ok && // received
718 ephPrev->IOD() == _newCorr->_iod) {
719 _newCorr->_eph = ephPrev;
720 switchToLastEph(ephLast, _newCorr);
721 }
722 else {
723#ifdef BNC_DEBUG_CMB
724 emit newMessage("bncComb: eph not found for " + prn.mid(0,3).toLatin1() +
725 QString(" with IOD %1").arg(_newCorr->_iod).toLatin1(), true);
726#endif
727 delete _newCorr; _newCorr = 0;
728 continue;
729 }
730 }
731
732 // Check satellite code biases
733 // ----------------------------
734 if (_satCodeBiases.contains(acName)) {
735 QMap<t_prn, t_satCodeBias>& storage = _satCodeBiases[acName];
736 if (storage.contains(clkCorr._prn)) {
737 _newCorr->_satCodeBias = storage[clkCorr._prn];
738 QMap<t_frequency::type, double> codeBiasesRefSig;
739 for (unsigned ii = 1; ii < cmbRefSig::cIF; ii++) {
740 t_frequency::type frqType = cmbRefSig::toFreq(sys, static_cast<cmbRefSig::type>(ii));
741 char frqNum = t_frequency::toString(frqType)[1];
742 char attrib = cmbRefSig::toAttrib(sys, static_cast<cmbRefSig::type>(ii));
743 QString rnxType2ch = QString("%1%2").arg(frqNum).arg(attrib);
744 for (unsigned ii = 0; ii < _newCorr->_satCodeBias._bias.size(); ii++) {
745 const t_frqCodeBias& bias = _newCorr->_satCodeBias._bias[ii];
746 if (rnxType2ch.toStdString() == bias._rnxType2ch) {
747 codeBiasesRefSig[frqType] = bias._value;
748 }
749 }
750 }
751 if (codeBiasesRefSig.size() == 2) {
752 map<t_frequency::type, double> codeCoeff;
753 double channel = double(_newCorr->_eph->slotNum());
754 cmbRefSig::coeff(sys, cmbRefSig::cIF, channel, codeCoeff);
755 map<t_frequency::type, double>::const_iterator it;
756 for (it = codeCoeff.begin(); it != codeCoeff.end(); it++) {
757 t_frequency::type frqType = it->first;
758 _newCorr->_satCodeBiasIF += it->second * codeBiasesRefSig[frqType];
759 }
760 }
761 _newCorr->_satCodeBias._bias.clear();
762 }
763 }
764
765 // Store correction into the buffer
766 // --------------------------------
767 QVector<cmbCorr*>& corrs = _buffer[sys].corrs;
768 QVectorIterator<cmbCorr*> itCorr(corrs);
769 bool available = false;
770 while (itCorr.hasNext()) {
771 cmbCorr* corr = itCorr.next();
772 QString prn = corr->_prn;
773 QString acName = corr->_acName;
774 if (_newCorr->_acName == acName &&
775 _newCorr->_prn == prn) {
776 available = true;
777 }
778 }
779
780 if (!available) {
781 corrs.push_back(_newCorr); _newCorr = 0;
782 }
783 else {
784 delete _newCorr; _newCorr = 0;
785 continue;
786 }
787 }
788
789 // Process Systems of this Epoch
790 // ------------------------------
791 QMapIterator<char, unsigned> itSys(_cmbSysPrn);
792 while (itSys.hasNext()) {
793 itSys.next();
794 char sys = itSys.key();
795 _log.clear();
796 QTextStream out(&_log, QIODevice::WriteOnly);
797 processSystem(epoTime, sys, out);
798 _buffer.remove(sys);
799 emit newMessage(_log, false);
800 }
801}
802
803void bncComb::processSystem(bncTime epoTime, char sys, QTextStream& out) {
804
805 out << "\n" << "Combination: " << sys << "\n"
806 << "--------------------------------" << "\n";
807
808 // Observation Statistics
809 // ----------------------
810 bool masterPresent = false;
811 QListIterator<cmbAC*> icAC(_ACs);
812 while (icAC.hasNext()) {
813 cmbAC* AC = icAC.next();
814 AC->numObs[sys] = 0;
815 QVectorIterator<cmbCorr*> itCorr(corrs(sys));
816 while (itCorr.hasNext()) {
817 cmbCorr* corr = itCorr.next();
818 if (corr->_acName == AC->name) {
819 AC->numObs[sys] += 1;
820 if (AC->name == _masterOrbitAC[sys]) {
821 masterPresent = true;
822 }
823 }
824 }
825 out << AC->name.toLatin1().data() << ": " << AC->numObs[sys] << "\n";
826 }
827
828 // If Master not present, switch to another one
829 // --------------------------------------------
830 const unsigned switchMasterAfterGap = 1;
831 if (masterPresent) {
832 _masterMissingEpochs[sys] = 0;
833 }
834 else {
835 ++_masterMissingEpochs[sys];
836 if (_masterMissingEpochs[sys] < switchMasterAfterGap) {
837 out << "Missing Master, Epoch skipped" << "\n";
838 _buffer.remove(sys);
839 emit newMessage(_log, false);
840 return;
841 }
842 else {
843 _masterMissingEpochs[sys] = 0;
844 QListIterator<cmbAC*> icAC(_ACs);
845 while (icAC.hasNext()) {
846 cmbAC* AC = icAC.next();
847 if (AC->numObs[sys] > 0) {
848 out << "Switching Master AC "
849 << _masterOrbitAC[sys].toLatin1().data() << " --> "
850 << AC->name.toLatin1().data() << " "
851 << epoTime.datestr().c_str() << " "
852 << epoTime.timestr().c_str() << "\n";
853 _masterOrbitAC[sys] = AC->name;
854 _masterIsAPC[sys] = AC->isAPC;
855 break;
856 }
857 }
858 }
859 }
860
861 QMap<QString, cmbCorr*> resCorr;
862
863 // Perform the actual Combination using selected Method
864 // ----------------------------------------------------
865 t_irc irc;
866 ColumnVector dx;
867 if (_method == filter) {
868 irc = processEpoch_filter(epoTime, sys, out, resCorr, dx);
869 }
870 else {
871 irc = processEpoch_singleEpoch(epoTime, sys, out, resCorr, dx);
872 }
873
874 // Update Parameter Values, Print Results
875 // --------------------------------------
876 if (irc == success) {
877 for (int iPar = 1; iPar <= _params[sys].size(); iPar++) {
878 cmbParam* pp = _params[sys][iPar-1];
879 pp->xx += dx(iPar);
880 if (pp->type == cmbParam::clkSat) {
881 if (resCorr.find(pp->prn) != resCorr.end()) {
882 // set clock result
883 resCorr[pp->prn]->_dClkResult = pp->xx / t_CST::c;
884 // Add Code Biases from SINEX File
885 if (_bsx) {
886 map<t_frequency::type, double> codeCoeff;
887 double channel = double(resCorr[pp->prn]->_eph->slotNum());
888 cmbRefSig::coeff(sys, cmbRefSig::cIF, channel, codeCoeff);
889 t_frequency::type fType1 = cmbRefSig::toFreq(sys, cmbRefSig::c1);
890 t_frequency::type fType2 = cmbRefSig::toFreq(sys, cmbRefSig::c2);
891 _bsx->determineSsrSatCodeBiases(pp->prn.mid(0,3), codeCoeff[fType1], codeCoeff[fType2], resCorr[pp->prn]->_satCodeBias);
892 }
893 }
894 }
895 out << epoTime.datestr().c_str() << " "
896 << epoTime.timestr().c_str() << " ";
897 out.setRealNumberNotation(QTextStream::FixedNotation);
898 out.setFieldWidth(8);
899 out.setRealNumberPrecision(4);
900 out << pp->toString(sys) << " "
901 << pp->xx << " +- " << sqrt(_QQ[sys](pp->index,pp->index)) << "\n";
902 out.setFieldWidth(0);
903 }
904 printResults(epoTime, out, resCorr);
905 dumpResults(epoTime, resCorr);
906 }
907}
908
909// Process Epoch - Filter Method
910////////////////////////////////////////////////////////////////////////////
911t_irc bncComb::processEpoch_filter(bncTime epoTime, char sys, QTextStream& out,
912 QMap<QString, cmbCorr*>& resCorr,
913 ColumnVector& dx) {
914
915 // Prediction Step
916 // ---------------
917 int nPar = _params[sys].size();
918 ColumnVector x0(nPar);
919 for (int iPar = 1; iPar <= nPar; iPar++) {
920 cmbParam* pp = _params[sys][iPar-1];
921 if (pp->epoSpec) {
922 pp->xx = 0.0;
923 _QQ[sys].Row(iPar) = 0.0;
924 _QQ[sys].Column(iPar) = 0.0;
925 _QQ[sys](iPar,iPar) = pp->sig0 * pp->sig0;
926 }
927 else {
928 _QQ[sys](iPar,iPar) += pp->sigP * pp->sigP;
929 }
930 x0(iPar) = pp->xx;
931 }
932
933 // Check Satellite Positions for Outliers
934 // --------------------------------------
935 if (checkOrbits(epoTime, sys, out) != success) {
936 return failure;
937 }
938
939 // Update and outlier detection loop
940 // ---------------------------------
941 SymmetricMatrix QQ_sav = _QQ[sys];
942 while (_running) {
943
944 Matrix AA;
945 ColumnVector ll;
946 DiagonalMatrix PP;
947
948 if (createAmat(sys, AA, ll, PP, x0, resCorr) != success) {
949 return failure;
950 }
951
952 dx.ReSize(nPar); dx = 0.0;
953 kalman(AA, ll, PP, _QQ[sys], dx);
954
955 ColumnVector vv = ll - AA * dx;
956
957 int maxResIndex;
958 double maxRes = vv.maximum_absolute_value1(maxResIndex);
959 out.setRealNumberNotation(QTextStream::FixedNotation);
960 out.setRealNumberPrecision(3);
961 out << epoTime.datestr().c_str() << " " << epoTime.timestr().c_str()
962 << " Maximum Residuum " << maxRes << ' '
963 << corrs(sys)[maxResIndex-1]->_acName << ' ' << corrs(sys)[maxResIndex-1]->_prn.mid(0,3);
964 if (maxRes > _MAXRES) {
965 for (int iPar = 1; iPar <= _params[sys].size(); iPar++) {
966 cmbParam* pp = _params[sys][iPar-1];
967 if (pp->type == cmbParam::offACSat &&
968 pp->AC == corrs(sys)[maxResIndex-1]->_acName &&
969 pp->prn == corrs(sys)[maxResIndex-1]->_prn.mid(0,3)) {
970 QQ_sav.Row(iPar) = 0.0;
971 QQ_sav.Column(iPar) = 0.0;
972 QQ_sav(iPar,iPar) = pp->sig0 * pp->sig0;
973 }
974 }
975
976 out << " Outlier" << "\n";
977 _QQ[sys] = QQ_sav;
978 delete corrs(sys)[maxResIndex-1];
979 corrs(sys).remove(maxResIndex-1);
980 }
981 else {
982 out << " OK" << "\n";
983 out.setRealNumberNotation(QTextStream::FixedNotation);
984 out.setRealNumberPrecision(4);
985 for (int ii = 0; ii < corrs(sys).size(); ii++) {
986 const cmbCorr* corr = corrs(sys)[ii];
987 out << epoTime.datestr().c_str() << ' '
988 << epoTime.timestr().c_str() << " "
989 << corr->_acName << ' ' << corr->_prn.mid(0,3);
990 out.setFieldWidth(10);
991 out << " res = " << vv[ii] << "\n";
992 out.setFieldWidth(0);
993 }
994 break;
995 }
996 }
997
998 return success;
999}
1000
1001// Print results
1002////////////////////////////////////////////////////////////////////////////
1003void bncComb::printResults(bncTime epoTime, QTextStream& out,
1004 const QMap<QString, cmbCorr*>& resCorr) {
1005
1006 QMapIterator<QString, cmbCorr*> it(resCorr);
1007 while (it.hasNext()) {
1008 it.next();
1009 cmbCorr* corr = it.value();
1010 const t_eph* eph = corr->_eph;
1011 if (eph) {
1012 ColumnVector xc(6);
1013 ColumnVector vv(3);
1014 if (eph->getCrd(epoTime, xc, vv, false) != success) {
1015 continue;
1016 }
1017 out << epoTime.datestr().c_str() << " "
1018 << epoTime.timestr().c_str() << " ";
1019 out.setFieldWidth(3);
1020 out << "Full Clock " << corr->_prn.mid(0,3) << " " << corr->_iod << " ";
1021 out.setFieldWidth(14);
1022 out << (xc(4) + corr->_dClkResult) * t_CST::c << "\n";
1023 out.setFieldWidth(0);
1024 }
1025 else {
1026 out << "bncComb::printResults bug" << "\n";
1027 }
1028 out.flush();
1029 }
1030}
1031
1032// Send results to RTNet Decoder and directly to PPP Client
1033////////////////////////////////////////////////////////////////////////////
1034void bncComb::dumpResults(bncTime epoTime, QMap<QString, cmbCorr*>& resCorr) {
1035
1036 QList<t_orbCorr> orbCorrections;
1037 QList<t_clkCorr> clkCorrections;
1038 QList<t_satCodeBias> satCodeBiasList;
1039
1040 unsigned year, month, day, hour, minute;
1041 double sec;
1042 epoTime.civil_date(year, month, day);
1043 epoTime.civil_time(hour, minute, sec);
1044
1045 QString outLines = QString().asprintf("* %4d %2d %2d %d %d %12.8f\n",
1046 year, month, day, hour, minute, sec);
1047
1048 QMutableMapIterator<QString, cmbCorr*> it(resCorr);
1049 while (it.hasNext()) {
1050 it.next();
1051 cmbCorr* corr = it.value();
1052
1053 // ORBIT
1054 t_orbCorr orbCorr(corr->_orbCorr);
1055 orbCorr._staID = "INTERNAL";
1056 orbCorrections.push_back(orbCorr);
1057
1058 // CLOCK
1059 t_clkCorr clkCorr(corr->_clkCorr);
1060 clkCorr._staID = "INTERNAL";
1061 clkCorr._dClk = corr->_dClkResult;
1062 clkCorr._dotDClk = 0.0;
1063 clkCorr._dotDotDClk = 0.0;
1064 clkCorrections.push_back(clkCorr);
1065
1066 ColumnVector xc(6);
1067 ColumnVector vv(3);
1068 corr->_eph->setClkCorr(dynamic_cast<const t_clkCorr*>(&clkCorr));
1069 corr->_eph->setOrbCorr(dynamic_cast<const t_orbCorr*>(&orbCorr));
1070 if (corr->_eph->getCrd(epoTime, xc, vv, true) != success) {
1071 delete corr;
1072 it.remove();
1073 continue;
1074 }
1075
1076 // Correction Phase Center --> CoM
1077 // -------------------------------
1078 ColumnVector dx(3); dx = 0.0;
1079 ColumnVector apc(3); apc = 0.0;
1080 ColumnVector com(3); com = 0.0;
1081 bool masterIsAPC = true;
1082 if (_antex) {
1083 double Mjd = epoTime.mjd() + epoTime.daysec()/86400.0;
1084 char sys = corr->_eph->prn().system();
1085 masterIsAPC = _masterIsAPC[sys];
1086 if (_antex->satCoMcorrection(corr->_prn, Mjd, xc.Rows(1,3), dx) != success) {
1087 dx = 0;
1088 _log += "antenna not found " + corr->_prn.mid(0,3).toLatin1() + '\n';
1089 }
1090 }
1091 if (masterIsAPC) {
1092 apc(1) = xc(1);
1093 apc(2) = xc(2);
1094 apc(3) = xc(3);
1095 com(1) = xc(1)-dx(1);
1096 com(2) = xc(2)-dx(2);
1097 com(3) = xc(3)-dx(3);
1098 }
1099 else {
1100 com(1) = xc(1);
1101 com(2) = xc(2);
1102 com(3) = xc(3);
1103 apc(1) = xc(1)+dx(1);
1104 apc(2) = xc(2)+dx(2);
1105 apc(3) = xc(3)+dx(3);
1106 }
1107
1108 outLines += corr->_prn.mid(0,3);
1109 QString hlp = QString().asprintf(
1110 " APC 3 %15.4f %15.4f %15.4f"
1111 " Clk 1 %15.4f"
1112 " Vel 3 %15.4f %15.4f %15.4f"
1113 " CoM 3 %15.4f %15.4f %15.4f",
1114 apc(1), apc(2), apc(3),
1115 xc(4) * t_CST::c,
1116 vv(1), vv(2), vv(3),
1117 com(1), com(2), com(3));
1118 outLines += hlp;
1119 hlp.clear();
1120
1121 // CODE BIASES
1122 if (corr->_satCodeBias._bias.size()) {
1123 t_satCodeBias satCodeBias(corr->_satCodeBias);
1124 satCodeBias._staID = "INTERNAL";
1125 satCodeBiasList.push_back(satCodeBias);
1126 hlp = QString().asprintf(" CodeBias %2lu", satCodeBias._bias.size());
1127 outLines += hlp;
1128 hlp.clear();
1129 for (unsigned ii = 0; ii < satCodeBias._bias.size(); ii++) {
1130 const t_frqCodeBias& frqCodeBias = satCodeBias._bias[ii];
1131 if (!frqCodeBias._rnxType2ch.empty()) {
1132 hlp = QString().asprintf(" %s%10.6f", frqCodeBias._rnxType2ch.c_str(), frqCodeBias._value);
1133 outLines += hlp;
1134 hlp.clear();
1135 }
1136 }
1137 }
1138 outLines += "\n";
1139 delete corr;
1140 it.remove();
1141 }
1142
1143 outLines += "EOE\n"; // End Of Epoch flag
1144 //cout << outLines.toStdString();
1145
1146 vector<string> errmsg;
1147 _rtnetDecoder->Decode(outLines.toLatin1().data(), outLines.length(), errmsg);
1148
1149 // Send new Corrections to PPP etc.
1150 // --------------------------------
1151 if (orbCorrections.size() > 0 && clkCorrections.size() > 0) {
1152 emit newOrbCorrections(orbCorrections);
1153 emit newClkCorrections(clkCorrections);
1154 }
1155 if (satCodeBiasList.size()) {
1156 emit newCodeBiases(satCodeBiasList);
1157 }
1158}
1159
1160// Create First Design Matrix and Vector of Measurements
1161////////////////////////////////////////////////////////////////////////////
1162t_irc bncComb::createAmat(char sys, Matrix& AA, ColumnVector& ll, DiagonalMatrix& PP,
1163 const ColumnVector& x0, QMap<QString, cmbCorr*>& resCorr) {
1164
1165 unsigned nPar = _params[sys].size();
1166 unsigned nObs = corrs(sys).size();
1167
1168 if (nObs == 0) {
1169 return failure;
1170 }
1171
1172 int maxSat = _cmbSysPrn[sys];
1173
1174 const int nCon = (_method == filter) ? 1 + maxSat : 0;
1175
1176 AA.ReSize(nObs+nCon, nPar); AA = 0.0;
1177 ll.ReSize(nObs+nCon); ll = 0.0;
1178 PP.ReSize(nObs+nCon); PP = 1.0 / (sigObs * sigObs);
1179
1180 int iObs = 0;
1181 QVectorIterator<cmbCorr*> itCorr(corrs(sys));
1182 while (itCorr.hasNext()) {
1183 cmbCorr* corr = itCorr.next();
1184 QString prn = corr->_prn;
1185
1186 ++iObs;
1187
1188 if (corr->_acName == _masterOrbitAC[sys] && resCorr.find(prn) == resCorr.end()) {
1189 resCorr[prn] = new cmbCorr(*corr);
1190 }
1191
1192 for (int iPar = 1; iPar <= _params[sys].size(); iPar++) {
1193 cmbParam* pp = _params[sys][iPar-1];
1194 AA(iObs, iPar) = pp->partial(sys, corr->_acName, prn);
1195 }
1196
1197 ll(iObs) = (corr->_clkCorr._dClk * t_CST::c - corr->_satCodeBiasIF) - DotProduct(AA.Row(iObs), x0);
1198
1199 PP(iObs, iObs) *= 1.0 / (corr->_weightFactor * corr->_weightFactor);
1200 }
1201
1202 // Regularization
1203 // --------------
1204 if (_method == filter) {
1205 const double Ph = 1.e6;
1206 PP(nObs+1) = Ph;
1207 for (int iPar = 1; iPar <= _params[sys].size(); iPar++) {
1208 cmbParam* pp = _params[sys][iPar-1];
1209 if ( AA.Column(iPar).maximum_absolute_value() > 0.0 &&
1210 pp->type == cmbParam::clkSat ) {
1211 AA(nObs+1, iPar) = 1.0;
1212 }
1213 }
1214 unsigned flag = 0;
1215 if (sys == 'E') {
1216 flag = 1;
1217 }
1218// if (sys == 'R') {
1219// return success;
1220// }
1221 int iCond = 1;
1222 // GNSS
1223 for (unsigned iGnss = 1; iGnss <= _cmbSysPrn[sys]; iGnss++) {
1224 QString prn = QString("%1%2_%3").arg(sys).arg(iGnss, 2, 10, QChar('0')).arg(flag);
1225 ++iCond;
1226 PP(nObs+iCond) = Ph;
1227 for (int iPar = 1; iPar <= _params[sys].size(); iPar++) {
1228 cmbParam* pp = _params[sys][iPar-1];
1229 if ( pp &&
1230 AA.Column(iPar).maximum_absolute_value() > 0.0 &&
1231 pp->type == cmbParam::offACSat &&
1232 pp->prn == prn) {
1233 AA(nObs+iCond, iPar) = 1.0;
1234 }
1235 }
1236 }
1237 }
1238
1239 return success;
1240}
1241
1242// Process Epoch - Single-Epoch Method
1243////////////////////////////////////////////////////////////////////////////
1244t_irc bncComb::processEpoch_singleEpoch(bncTime epoTime, char sys, QTextStream& out,
1245 QMap<QString, cmbCorr*>& resCorr,
1246 ColumnVector& dx) {
1247
1248 // Check Satellite Positions for Outliers
1249 // --------------------------------------
1250 if (checkOrbits(epoTime, sys, out) != success) {
1251 return failure;
1252 }
1253
1254 // Outlier Detection Loop
1255 // ----------------------
1256 while (_running) {
1257
1258 // Remove Satellites that are not in Master
1259 // ----------------------------------------
1260 QMutableVectorIterator<cmbCorr*> it(corrs(sys));
1261 while (it.hasNext()) {
1262 cmbCorr* corr = it.next();
1263 QString prn = corr->_prn;
1264 bool foundMaster = false;
1265 QVectorIterator<cmbCorr*> itHlp(corrs(sys));
1266 while (itHlp.hasNext()) {
1267 cmbCorr* corrHlp = itHlp.next();
1268 QString prnHlp = corrHlp->_prn;
1269 QString ACHlp = corrHlp->_acName;
1270 if (ACHlp == _masterOrbitAC[sys] && prn == prnHlp) {
1271 foundMaster = true;
1272 break;
1273 }
1274 }
1275 if (!foundMaster) {
1276 delete corr;
1277 it.remove();
1278 }
1279 }
1280
1281 // Count Number of Observations per Satellite and per AC
1282 // -----------------------------------------------------
1283 QMap<QString, int> numObsPrn;
1284 QMap<QString, int> numObsAC;
1285 QVectorIterator<cmbCorr*> itCorr(corrs(sys));
1286 while (itCorr.hasNext()) {
1287 cmbCorr* corr = itCorr.next();
1288 QString prn = corr->_prn;
1289 QString AC = corr->_acName;
1290 if (numObsPrn.find(prn) == numObsPrn.end()) {
1291 numObsPrn[prn] = 1;
1292 }
1293 else {
1294 numObsPrn[prn] += 1;
1295 }
1296 if (numObsAC.find(AC) == numObsAC.end()) {
1297 numObsAC[AC] = 1;
1298 }
1299 else {
1300 numObsAC[AC] += 1;
1301 }
1302 }
1303
1304 // Clean-Up the Parameters
1305 // -----------------------
1306 for (int iPar = 1; iPar <= _params[sys].size(); iPar++) {
1307 delete _params[sys][iPar-1];
1308 }
1309 _params[sys].clear();
1310
1311 // Set new Parameters
1312 // ------------------
1313 int nextPar = 0;
1314
1315 QMapIterator<QString, int> itAC(numObsAC);
1316 while (itAC.hasNext()) {
1317 itAC.next();
1318 const QString& AC = itAC.key();
1319 int numObs = itAC.value();
1320 if (AC != _masterOrbitAC[sys] && numObs > 0) {
1321 _params[sys].push_back(new cmbParam(cmbParam::offACgnss, ++nextPar, AC, ""));
1322 }
1323 }
1324
1325 QMapIterator<QString, int> itPrn(numObsPrn);
1326 while (itPrn.hasNext()) {
1327 itPrn.next();
1328 const QString& prn = itPrn.key();
1329 int numObs = itPrn.value();
1330 if (numObs > 0) {
1331 _params[sys].push_back(new cmbParam(cmbParam::clkSat, ++nextPar, "", prn));
1332 }
1333 }
1334
1335 int nPar = _params[sys].size();
1336 ColumnVector x0(nPar);
1337 x0 = 0.0;
1338
1339 // Create First-Design Matrix
1340 // --------------------------
1341 Matrix AA;
1342 ColumnVector ll;
1343 DiagonalMatrix PP;
1344 if (createAmat(sys, AA, ll, PP, x0, resCorr) != success) {
1345 return failure;
1346 }
1347
1348 ColumnVector vv;
1349 try {
1350 Matrix ATP = AA.t() * PP;
1351 SymmetricMatrix NN; NN << ATP * AA;
1352 ColumnVector bb = ATP * ll;
1353 _QQ[sys] = NN.i();
1354 dx = _QQ[sys] * bb;
1355 vv = ll - AA * dx;
1356 }
1357 catch (Exception& exc) {
1358 out << exc.what() << "\n";
1359 return failure;
1360 }
1361
1362 int maxResIndex;
1363 double maxRes = vv.maximum_absolute_value1(maxResIndex);
1364 out.setRealNumberNotation(QTextStream::FixedNotation);
1365 out.setRealNumberPrecision(3);
1366 out << epoTime.datestr().c_str() << " " << epoTime.timestr().c_str()
1367 << " Maximum Residuum " << maxRes << ' '
1368 << corrs(sys)[maxResIndex-1]->_acName << ' ' << corrs(sys)[maxResIndex-1]->_prn.mid(0,3);
1369
1370 if (maxRes > _MAXRES) {
1371 out << " Outlier" << "\n";
1372 delete corrs(sys)[maxResIndex-1];
1373 corrs(sys).remove(maxResIndex-1);
1374 }
1375 else {
1376 out << " OK" << "\n";
1377 out.setRealNumberNotation(QTextStream::FixedNotation);
1378 out.setRealNumberPrecision(3);
1379 for (int ii = 0; ii < vv.Nrows(); ii++) {
1380 const cmbCorr* corr = corrs(sys)[ii];
1381 out << epoTime.datestr().c_str() << ' '
1382 << epoTime.timestr().c_str() << " "
1383 << corr->_acName << ' ' << corr->_prn.mid(0,3);
1384 out.setFieldWidth(6);
1385 out << " res = " << vv[ii] << "\n";
1386 out.setFieldWidth(0);
1387 }
1388 return success;
1389 }
1390
1391 }
1392
1393 return failure;
1394}
1395
1396// Check Satellite Positions for Outliers
1397////////////////////////////////////////////////////////////////////////////
1398t_irc bncComb::checkOrbits(bncTime epoTime, char sys, QTextStream& out) {
1399
1400 const double MAX_DISPLACEMENT_INIT = 0.50;
1401
1402 double MAX_DISPLACEMENT = MAX_DISPLACEMENT_INIT;
1403 if (sys == 'C') {
1404 MAX_DISPLACEMENT *= 10.0;
1405 }
1406 if (sys == 'R') {
1407 MAX_DISPLACEMENT *= 2.0;
1408 }
1409
1410 // Switch to last ephemeris (if possible)
1411 // --------------------------------------
1412 QMutableVectorIterator<cmbCorr*> im(corrs(sys));
1413 while (im.hasNext()) {
1414 cmbCorr* corr = im.next();
1415 QString prn = corr->_prn;
1416
1417 t_eph* ephLast = _ephUser.ephLast(prn);
1418 t_eph* ephPrev = _ephUser.ephPrev(prn);
1419
1420 if (ephLast == 0) {
1421 out << "checkOrbit: missing eph (not found) " << corr->_prn.mid(0,3) << "\n";
1422 delete corr;
1423 im.remove();
1424 }
1425 else if (corr->_eph == 0) {
1426 out << "checkOrbit: missing eph (zero) " << corr->_prn.mid(0,3) << "\n";
1427 delete corr;
1428 im.remove();
1429 }
1430 else {
1431 if ( corr->_eph == ephLast || corr->_eph == ephPrev ) {
1432 switchToLastEph(ephLast, corr);
1433 }
1434 else {
1435 out << "checkOrbit: missing eph (deleted) " << corr->_prn.mid(0,3) << "\n";
1436 delete corr;
1437 im.remove();
1438 }
1439 }
1440 }
1441
1442 while (_running) {
1443
1444 // Compute Mean Corrections for all Satellites
1445 // -------------------------------------------
1446 QMap<QString, int> numCorr;
1447 QMap<QString, ColumnVector> meanRao;
1448 QVectorIterator<cmbCorr*> it(corrs(sys));
1449 while (it.hasNext()) {
1450 cmbCorr* corr = it.next();
1451 QString prn = corr->_prn;
1452 if (meanRao.find(prn) == meanRao.end()) {
1453 meanRao[prn].ReSize(4);
1454 meanRao[prn].Rows(1,3) = corr->_orbCorr._xr;
1455 meanRao[prn](4) = 1;
1456 }
1457 else {
1458 meanRao[prn].Rows(1,3) += corr->_orbCorr._xr;
1459 meanRao[prn](4) += 1;
1460 }
1461 if (numCorr.find(prn) == numCorr.end()) {
1462 numCorr[prn] = 1;
1463 }
1464 else {
1465 numCorr[prn] += 1;
1466 }
1467 }
1468
1469 // Compute Differences wrt. Mean, find Maximum
1470 // -------------------------------------------
1471 QMap<QString, cmbCorr*> maxDiff;
1472 it.toFront();
1473 while (it.hasNext()) {
1474 cmbCorr* corr = it.next();
1475 QString prn = corr->_prn;
1476 if (meanRao[prn](4) != 0) {
1477 meanRao[prn] /= meanRao[prn](4);
1478 meanRao[prn](4) = 0;
1479 }
1480 corr->_diffRao = corr->_orbCorr._xr - meanRao[prn].Rows(1,3);
1481 if (maxDiff.find(prn) == maxDiff.end()) {
1482 maxDiff[prn] = corr;
1483 }
1484 else {
1485 double normMax = maxDiff[prn]->_diffRao.NormFrobenius();
1486 double norm = corr->_diffRao.NormFrobenius();
1487 if (norm > normMax) {
1488 maxDiff[prn] = corr;
1489 }
1490 }
1491 }
1492
1493 if (_ACs.size() == 1) {
1494 break;
1495 }
1496
1497 // Remove Outliers
1498 // ---------------
1499 bool removed = false;
1500 QMutableVectorIterator<cmbCorr*> im(corrs(sys));
1501 while (im.hasNext()) {
1502 cmbCorr* corr = im.next();
1503 QString prn = corr->_prn;
1504 if (numCorr[prn] < 2) {
1505 delete corr;
1506 im.remove();
1507 }
1508 else if (corr == maxDiff[prn]) {
1509 double norm = corr->_diffRao.NormFrobenius();
1510 if (norm > MAX_DISPLACEMENT) {
1511 out << epoTime.datestr().c_str() << " "
1512 << epoTime.timestr().c_str() << " "
1513 << "Orbit Outlier: "
1514 << corr->_acName.toLatin1().data() << " "
1515 << prn.mid(0,3).toLatin1().data() << " "
1516 << corr->_iod << " "
1517 << norm << "\n";
1518 delete corr;
1519 im.remove();
1520 removed = true;
1521 }
1522 }
1523 }
1524
1525 if (!removed) {
1526 break;
1527 }
1528 }
1529
1530 return success;
1531}
1532
1533//
1534////////////////////////////////////////////////////////////////////////////
1535void bncComb::slotProviderIDChanged(QString mountPoint) {
1536 QMutexLocker locker(&_mutex);
1537
1538 QTextStream out(&_log, QIODevice::WriteOnly);
1539
1540 // Find the AC Name
1541 // ----------------
1542 QString acName;
1543 QListIterator<cmbAC*> icAC(_ACs);
1544 while (icAC.hasNext()) {
1545 cmbAC *AC = icAC.next();
1546 if (AC->mountPoint == mountPoint) {
1547 acName = AC->name;
1548 out << "Provider ID changed: AC " << AC->name.toLatin1().data() << " "
1549 << _resTime.datestr().c_str() << " " << _resTime.timestr().c_str()
1550 << "\n";
1551 break;
1552 }
1553 }
1554 if (acName.isEmpty()) {
1555 return;
1556 }
1557 QMapIterator<char, unsigned> itSys(_cmbSysPrn);
1558 while (itSys.hasNext()) {
1559 itSys.next();
1560 char sys = itSys.key();
1561 // Remove all corrections of the corresponding AC
1562 // ----------------------------------------------
1563 QVector<cmbCorr*> &corrVec = _buffer[sys].corrs;
1564 QMutableVectorIterator<cmbCorr*> it(corrVec);
1565 while (it.hasNext()) {
1566 cmbCorr *corr = it.next();
1567 if (acName == corr->_acName) {
1568 delete corr;
1569 it.remove();
1570 }
1571 }
1572 // Reset Satellite Offsets
1573 // -----------------------
1574 if (_method == filter) {
1575 for (int iPar = 1; iPar <= _params[sys].size(); iPar++) {
1576 cmbParam *pp = _params[sys][iPar - 1];
1577 if (pp->AC == acName && pp->type == cmbParam::offACSat) {
1578 pp->xx = 0.0;
1579 _QQ[sys].Row(iPar) = 0.0;
1580 _QQ[sys].Column(iPar) = 0.0;
1581 _QQ[sys](iPar, iPar) = pp->sig0 * pp->sig0;
1582 }
1583 }
1584 }
1585 }
1586}
Note: See TracBrowser for help on using the repository browser.