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

Last change on this file since 10694 was 10694, checked in by stuerze, 6 weeks ago

some corrections are added for consistency before clock combination
File size: 51.6 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").toString().split("sec").first().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 _useNavic = (Qt::CheckState(settings.value("cmbNavic").toInt()) == Qt::Checked) ? true : false;
165 if (_useNavic) {
166 _cmbSysPrn['I'] = t_prn::MAXPRN_NavIC;
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->excludeSats = hlp[3].split(QRegExp("[ ,]"), Qt::SkipEmptyParts);
179 newAC->isAPC = bool(newAC->mountPoint.mid(0,4) == "SSRA");
180 QMapIterator<char, unsigned> itSys(_cmbSysPrn);
181 // init
182 while (itSys.hasNext()) {
183 itSys.next();
184 char sys = itSys.key();
185 if (!_masterOrbitAC.contains(sys) &&
186 !newAC->excludeSats.contains(QString(sys), Qt::CaseSensitive)) {
187 _masterOrbitAC[sys] = newAC->name;
188 _masterIsAPC[sys] = newAC->isAPC;
189 }
190 }
191 _ACs.append(newAC);
192 }
193 }
194
195 QString ssrFormat;
196 _ssrCorr = 0;
197 QListIterator<QString> it(settings.value("uploadMountpointsOut").toStringList());
198 while (it.hasNext()) {
199 QStringList hlp = it.next().split(",");
200 if (hlp.size() > 7) {
201 ssrFormat = hlp[7];
202 }
203 }
204 if (ssrFormat == "IGS-SSR") {
205 _ssrCorr = new SsrCorrIgs();
206 }
207 else if (ssrFormat == "RTCM-SSR") {
208 _ssrCorr = new SsrCorrRtcm();
209 }
210 else { // default
211 _ssrCorr = new SsrCorrIgs();
212 }
213
214 _rtnetDecoder = new bncRtnetDecoder();
215
216 connect(this, SIGNAL(newMessage(QByteArray,bool)), BNC_CORE, SLOT(slotMessage(const QByteArray,bool)));
217 connect(BNC_CORE, SIGNAL(providerIDChanged(QString)), this, SLOT(slotProviderIDChanged(QString)));
218 connect(BNC_CORE, SIGNAL(newOrbCorrections(QList<t_orbCorr>)), this, SLOT(slotNewOrbCorrections(QList<t_orbCorr>)));
219 connect(BNC_CORE, SIGNAL(newClkCorrections(QList<t_clkCorr>)), this, SLOT(slotNewClkCorrections(QList<t_clkCorr>)));
220 connect(BNC_CORE, SIGNAL(newCodeBiases(QList<t_satCodeBias>)), this, SLOT(slotNewCodeBiases(QList<t_satCodeBias>)));
221 connect(BNC_CORE, SIGNAL(newPhaseBiases(QList<t_satPhaseBias>)), this, SLOT(slotNewPhaseBiases(QList<t_satPhaseBias>)));
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 int maxPrn = itSys.value();
242 // AC
243 QListIterator<cmbAC*> itAc(_ACs);
244 while (itAc.hasNext()) {
245 cmbAC* AC = itAc.next();
246 _params[sys].push_back(new cmbParam(cmbParam::offACgnss, ++nextPar, AC->name, ""));
247 for (int iGnss = 1; iGnss <= maxPrn; iGnss++) {
248 int flag = t_corrSSR::getSsrNavTypeFlag(sys, 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 (int iGnss = 1; iGnss <= maxPrn; iGnss++) {
254 int flag = t_corrSSR::getSsrNavTypeFlag(sys, iGnss);
255 QString prn = QString("%1%2_%3").arg(sys).arg(iGnss, 2, 10, QChar('0')).arg(flag);
256 _params[sys].push_back(new cmbParam(cmbParam::clkSat, ++nextPar, "", prn));
257 }
258 // Initialize Variance-Covariance Matrix
259 // -------------------------------------
260 _QQ[sys].ReSize(_params[sys].size());
261 _QQ[sys] = 0.0;
262 for (int iPar = 1; iPar <= _params[sys].size(); iPar++) {
263 cmbParam* pp = _params[sys][iPar-1];
264 _QQ[sys](iPar,iPar) = pp->sig0 * pp->sig0;
265 }
266 }
267 }
268
269 // ANTEX File
270 // ----------
271 _antex = 0;
272 QString antexFileName = settings.value("uploadAntexFile").toString();
273 if (!antexFileName.isEmpty()) {
274 _antex = new bncAntex();
275 if (_antex->readFile(antexFileName) != success) {
276 emit newMessage("bncComb: wrong ANTEX file", true);
277 delete _antex;
278 _antex = 0;
279 }
280 }
281
282
283 // Bias SINEX File
284 // ---------------
285 _bsx = 0;
286 QString bsxFileName = settings.value("cmbBsxFile").toString();
287 if (!bsxFileName.isEmpty()) {
288 _bsx = new bncBiasSnx();
289 if (_bsx->readFile(bsxFileName) != success) {
290 emit newMessage("bncComb: wrong Bias SINEX file", true);
291 delete _bsx;
292 _bsx = 0;
293 }
294 }
295 if (_bsx) {
296 _ms = 3.0 * 3600 * 1000.0;
297 QTimer::singleShot(_ms, this, SLOT(slotReadBiasSnxFile()));
298 }
299
300 // Maximal Residuum
301 // ----------------
302 _MAX_RES = settings.value("cmbMaxres").toDouble();
303 if (_MAX_RES <= 0.0) {
304 _MAX_RES = 999.0;
305 }
306
307 // Maximal Displacement
308 // --------------------
309 _MAX_DISPLACEMENT = settings.value("cmbMaxdisplacement").toDouble();
310 if (_MAX_DISPLACEMENT <= 0.0) {
311 _MAX_DISPLACEMENT = 2.0;
312 }
313
314 // Logfile
315 // -------
316 QString intr = "1 day";
317 QString logFileCmb = settings.value("cmbLogpath").toString();
318 int l = logFileCmb.length();
319 if (logFileCmb.isEmpty()) {
320 _logFile = 0;
321 }
322 else {
323 if (l && logFileCmb[l-1] != QDir::separator() ) {
324 logFileCmb += QDir::separator();
325 }
326
327 logFileCmb = logFileCmb + "COMB00BNC" + "${V3PROD}" + ".CMB";
328
329 _logFile = new bncoutf(logFileCmb, intr, _cmbSampl);
330 }
331
332 _newCorr = 0;
333}
334
335// Destructor
336////////////////////////////////////////////////////////////////////////////
337bncComb::~bncComb() {
338
339 _running = false;
340#ifndef WIN32
341 sleep(2);
342#else
343 Sleep(2000);
344#endif
345
346 QListIterator<cmbAC*> icAC(_ACs);
347 while (icAC.hasNext()) {
348 delete icAC.next();
349 }
350 _ACs.clear();
351
352 delete _rtnetDecoder;
353
354 if (_ssrCorr) {
355 delete _ssrCorr;
356 }
357
358 if (_newCorr) {
359 delete _newCorr;
360 }
361
362 delete _antex;
363 delete _bsx;
364 delete _logFile;
365
366 QMapIterator<char, unsigned> itSys(_cmbSysPrn);
367 while (itSys.hasNext()) {
368 itSys.next();
369 char sys = itSys.key();
370 for (int iPar = 1; iPar <= _params[sys].size(); iPar++) {
371 delete _params[sys][iPar-1];
372 }
373 _buffer.remove(sys);
374 }
375 _params.clear();
376 _buffer.clear();
377 _cmbSysPrn.clear();
378
379 while (!_epoClkData.empty()) {
380 delete _epoClkData.front();
381 _epoClkData.pop_front();
382 }
383}
384
385void bncComb::slotReadBiasSnxFile() {
386 bncSettings settings;
387 QString bsxFileName = settings.value("cmbBsxFile").toString();
388
389 if (bsxFileName.isEmpty()) {
390 emit newMessage("bncComb: no Bias SINEX file specified", true);
391 return;
392 }
393 if (!_bsx) {
394 _bsx = new bncBiasSnx();
395 }
396 if (_bsx->readFile(bsxFileName) != success) {
397 emit newMessage("bncComb: wrong Bias SINEX file", true);
398 delete _bsx;
399 _bsx = 0;
400 }
401 else {
402 emit newMessage("bncComb: successfully read Bias SINEX file", true);
403 }
404
405 QTimer::singleShot(_ms, this, SLOT(slotReadBiasSnxFile()));
406}
407
408
409// Remember orbit corrections
410////////////////////////////////////////////////////////////////////////////
411void bncComb::slotNewOrbCorrections(QList<t_orbCorr> orbCorrections) {
412 QMutexLocker locker(&_mutex);
413 for (int ii = 0; ii < orbCorrections.size(); ii++) {
414 t_orbCorr& orbCorr = orbCorrections[ii];
415 QString staID(orbCorr._staID.c_str());
416 char sys = orbCorr._prn.system();
417
418 if (!_cmbSysPrn.contains(sys)){
419 continue;
420 }
421
422 // Find/Check the AC Name
423 // ----------------------
424 QString acName;
425 QStringList excludeSats;
426 QListIterator<cmbAC*> icAC(_ACs);
427 while (icAC.hasNext()) {
428 cmbAC* AC = icAC.next();
429 if (AC->mountPoint == staID) {
430 acName = AC->name;
431 excludeSats = AC->excludeSats;
432 break;
433 }
434 }
435 if (acName.isEmpty() || excludeSat(orbCorr._prn, excludeSats)) {
436 continue;
437 }
438
439 // Store the correction
440 // --------------------
441 QMap<t_prn, t_orbCorr>& storage = _orbCorrections[acName];
442 storage[orbCorr._prn] = orbCorr;
443 }
444}
445
446// Remember Satellite Code Biases
447////////////////////////////////////////////////////////////////////////////
448void bncComb::slotNewCodeBiases(QList<t_satCodeBias> satCodeBiases) {
449 QMutexLocker locker(&_mutex);
450
451 for (int ii = 0; ii < satCodeBiases.size(); ii++) {
452 t_satCodeBias& satCodeBias = satCodeBiases[ii];
453 QString staID(satCodeBias._staID.c_str());
454 char sys = satCodeBias._prn.system();
455
456 if (!_cmbSysPrn.contains(sys)){
457 continue;
458 }
459
460 // Find/Check the AC Name
461 // ----------------------
462 QString acName;
463 QStringList excludeSats;
464 QListIterator<cmbAC*> icAC(_ACs);
465 while (icAC.hasNext()) {
466 cmbAC* AC = icAC.next();
467 if (AC->mountPoint == staID) {
468 acName = AC->name;
469 excludeSats = AC->excludeSats;
470 break;
471 }
472 }
473 if (acName.isEmpty() || excludeSat(satCodeBias._prn, excludeSats)) {
474 continue;
475 }
476
477 // Store the correction
478 // --------------------
479 QMap<t_prn, t_satCodeBias>& storage = _satCodeBiases[acName];
480 storage[satCodeBias._prn] = satCodeBias;
481 }
482 return;
483}
484
485// Remember Satellite Phase Biases
486////////////////////////////////////////////////////////////////////////////
487void bncComb::slotNewPhaseBiases(QList<t_satPhaseBias> satPhaseBiases) {
488 QMutexLocker locker(&_mutex);
489
490 for (int ii = 0; ii < satPhaseBiases.size(); ii++) {
491 t_satPhaseBias& satPhaseBias = satPhaseBiases[ii];
492 QString staID(satPhaseBias._staID.c_str());
493 char sys = satPhaseBias._prn.system();
494
495 if (!_cmbSysPrn.contains(sys)){
496 continue;
497 }
498
499 // Find/Check the AC Name
500 // ----------------------
501 QString acName;
502 QStringList excludeSats;
503 QListIterator<cmbAC*> icAC(_ACs);
504 while (icAC.hasNext()) {
505 cmbAC* AC = icAC.next();
506 if (AC->mountPoint == staID) {
507 acName = AC->name;
508 excludeSats = AC->excludeSats;
509 break;
510 }
511 }
512 if (acName.isEmpty() || excludeSat(satPhaseBias._prn, excludeSats)) {
513 continue;
514 }
515
516 // Store the correction
517 // --------------------
518 QMap<t_prn, t_satPhaseBias>& storage = _satPhaseBiases[acName];
519 storage[satPhaseBias._prn] = satPhaseBias;
520 }
521 return;
522}
523
524
525// Process clock corrections
526////////////////////////////////////////////////////////////////////////////
527void bncComb::slotNewClkCorrections(QList<t_clkCorr> clkCorrections) {
528 QMutexLocker locker(&_mutex);
529 const double outWait = 1.0 * _cmbSampl;
530 int currentWeek = 0;
531 double currentSec = 0.0;
532 currentGPSWeeks(currentWeek, currentSec);
533 bncTime currentTime(currentWeek, currentSec);
534
535 // Loop over all observations (possible different epochs)
536 // -----------------------------------------------------
537 for (int ii = 0; ii < clkCorrections.size(); ii++) {
538 t_clkCorr& newClk = clkCorrections[ii];
539 char sys = newClk._prn.system();
540 if (!_cmbSysPrn.contains(sys)){
541 continue;
542 }
543
544 // Check Modulo Time
545 // -----------------
546 int sec = int(nint(newClk._time.gpssec()*10));
547 if (sec % (_cmbSampl*10) != 0.0) {
548 continue;
549 }
550
551 // Find/Check the AC Name
552 // ----------------------
553 QString acName;
554 QStringList excludeSats;
555 bool isAPC;
556 QString staID(newClk._staID.c_str());
557 QListIterator<cmbAC*> icAC(_ACs);
558 while (icAC.hasNext()) {
559 cmbAC* AC = icAC.next();
560 if (AC->mountPoint == staID) {
561 acName = AC->name;
562 excludeSats = AC->excludeSats;
563 isAPC = AC->isAPC;
564 break;
565 }
566 }
567 if (acName.isEmpty() ||
568 isAPC != _masterIsAPC[sys] ||
569 excludeSat(newClk._prn, excludeSats)) {
570 continue;
571 }
572
573 // Check regarding current time
574 // ----------------------------
575 if (BNC_CORE->mode() != t_bncCore::batchPostProcessing) {
576 if (fabs(currentTime - newClk._time) > 60.0) { // future or very old data sets
577 #ifdef BNC_DEBUG_CMB
578 emit newMessage("bncComb: future or very old data sets: " + acName.toLatin1() + " " + newClk._prn.toString().c_str() +
579 QString(" %1 ").arg(newClk._time.timestr().c_str()).toLatin1() + "vs. current time" +
580 QString(" %1 ").arg(currentTime.timestr().c_str()).toLatin1(), true);
581 #endif
582 continue;
583 }
584 }
585
586 // Check Correction Age
587 // --------------------
588 if (_resTime.valid() && newClk._time <= _resTime) {
589#ifdef BNC_DEBUG_CMB
590 emit newMessage("bncComb: old correction: " + acName.toLatin1() + " " + newClk._prn.toString().c_str() +
591 QString(" %1 ").arg(newClk._time.timestr().c_str()).toLatin1() + "vs. last processing Epoch" +
592 QString(" %1 ").arg(_resTime.timestr().c_str()).toLatin1(), true);
593#endif
594 continue;
595 }
596
597 // Set the last time
598 // -----------------
599 if (_lastClkCorrTime.undef() || newClk._time > _lastClkCorrTime) {
600 _lastClkCorrTime = newClk._time;
601 }
602
603 // Find the corresponding data epoch or create a new one
604 // -----------------------------------------------------
605 epoClkData* epoch = 0;
606 deque<epoClkData*>::const_iterator it;
607 for (it = _epoClkData.begin(); it != _epoClkData.end(); it++) {
608 if (newClk._time == (*it)->_time) {
609 epoch = *it;
610 break;
611 }
612 }
613 if (epoch == 0) {
614 if (_epoClkData.empty() || newClk._time > _epoClkData.back()->_time) {
615 epoch = new epoClkData;
616 epoch->_time = newClk._time;
617 _epoClkData.push_back(epoch);
618 }
619 }
620
621 // Put data into the epoch
622 // -----------------------
623 if (epoch != 0) {
624 epoch->_clkCorr.push_back(newClk);
625 }
626 }
627
628 // Make sure the buffer does not grow beyond any limit
629 // ---------------------------------------------------
630 const unsigned MAX_EPODATA_SIZE = 60.0 / _cmbSampl;
631 if (_epoClkData.size() > MAX_EPODATA_SIZE) {
632 delete _epoClkData.front();
633 _epoClkData.pop_front();
634 }
635
636 // Process the oldest epochs
637 // ------------------------
638 while (_epoClkData.size()) {
639
640 if (!_lastClkCorrTime.valid()) {
641 return;
642 }
643
644 const vector<t_clkCorr>& clkCorrVec = _epoClkData.front()->_clkCorr;
645
646 bncTime epoTime = _epoClkData.front()->_time;
647
648 if (BNC_CORE->mode() != t_bncCore::batchPostProcessing) {
649 if ((currentTime - epoTime) > 60.0) {
650 delete _epoClkData.front();
651 _epoClkData.pop_front();
652 continue;
653 }
654 }
655 // Process the front epoch
656 // -----------------------
657 if (epoTime < (_lastClkCorrTime - outWait)) {
658 _resTime = epoTime;
659 processEpoch(_resTime, clkCorrVec);
660 delete _epoClkData.front();
661 _epoClkData.pop_front();
662 }
663 else {
664 return;
665 }
666 }
667}
668
669// Change the correction so that it refers to last received ephemeris
670////////////////////////////////////////////////////////////////////////////
671void bncComb::switchToLastEph(t_eph* lastEph, cmbCorr* corr) {
672
673 if (corr->_eph == lastEph) {
674 return;
675 }
676
677 ColumnVector oldXC(6);
678 ColumnVector oldVV(3);
679 if (corr->_eph->getCrd(corr->_time, oldXC, oldVV, false) != success) {
680 return;
681 }
682
683 ColumnVector newXC(6);
684 ColumnVector newVV(3);
685 if (lastEph->getCrd(corr->_time, newXC, newVV, false) != success) {
686 return;
687 }
688
689 ColumnVector dX = newXC.Rows(1,3) - oldXC.Rows(1,3);
690 ColumnVector dV = newVV - oldVV;
691 double dC = newXC(4) - oldXC(4);
692
693 ColumnVector dRAO(3);
694 XYZ_to_RSW(newXC.Rows(1,3), newVV, dX, dRAO);
695
696 ColumnVector dDotRAO(3);
697 XYZ_to_RSW(newXC.Rows(1,3), newVV, dV, dDotRAO);
698
699 QString msg = "bncComb: switch corr " + corr->_prn.mid(0,3)
700 + QString(" %1 -> %2 %3").arg(corr->_iod,3).arg(lastEph->IOD(),3).arg(dC*t_CST::c, 8, 'f', 4);
701
702 emit newMessage(msg.toLatin1(), false);
703
704 corr->_iod = lastEph->IOD();
705 corr->_eph = lastEph;
706
707 corr->_orbCorr._xr += dRAO;
708 corr->_orbCorr._dotXr += dDotRAO;
709 corr->_clkCorr._dClk -= dC;
710}
711
712// Process Epoch
713////////////////////////////////////////////////////////////////////////////
714void bncComb::processEpoch(bncTime epoTime, const vector<t_clkCorr>& clkCorrVec) {
715 QDateTime now = currentDateAndTimeGPS();
716 bncTime currentTime(now.toString(Qt::ISODate).toStdString());
717 for (unsigned ii = 0; ii < clkCorrVec.size(); ii++) {
718 const t_clkCorr& clkCorr = clkCorrVec[ii];
719 QString staID(clkCorr._staID.c_str());
720 QString prnStr(clkCorr._prn.toInternalString().c_str());
721 char sys = clkCorr._prn.system();
722
723 // Find/Check the AC Name
724 // ----------------------
725 QString acName;
726 double weigthFactor;
727 QListIterator<cmbAC*> icAC(_ACs);
728 while (icAC.hasNext()) {
729 cmbAC* AC = icAC.next();
730 if (AC->mountPoint == staID) {
731 acName = AC->name;
732 weigthFactor = AC->weightFactor;
733 break;
734 }
735 }
736
737 // Create new correction
738 // ---------------------
739 _newCorr = new cmbCorr();
740 _newCorr->_prn = prnStr;
741 _newCorr->_time = clkCorr._time;
742 _newCorr->_iod = clkCorr._iod;
743 _newCorr->_acName = acName;
744 _newCorr->_weightFactor = weigthFactor;
745 _newCorr->_clkCorr = clkCorr;
746
747 // Check orbit correction
748 // ----------------------
749 if (!_orbCorrections.contains(acName)) {
750 delete _newCorr; _newCorr = 0;
751 continue;
752 }
753 else {
754 QMap<t_prn, t_orbCorr>& storage = _orbCorrections[acName];
755 if (!storage.contains(clkCorr._prn) ||
756 storage[clkCorr._prn]._iod != _newCorr->_iod) {
757 delete _newCorr; _newCorr = 0;
758 continue;
759 }
760 else {
761 _newCorr->_orbCorr = storage[clkCorr._prn];
762 }
763 }
764
765 // Check the Ephemeris
766 //--------------------
767 t_eph* ephLast = _ephUser.ephLast(prnStr);
768 t_eph* ephPrev = _ephUser.ephPrev(prnStr);
769 if (ephLast == 0) {
770#ifdef BNC_DEBUG_CMB
771 emit newMessage("bncComb: eph not found for " + prnStr.mid(0,3).toLatin1(), true);
772#endif
773 delete _newCorr; _newCorr = 0;
774 continue;
775 }
776 else if (outDatedBcep(ephLast, currentTime) ||
777 ephLast->checkState() == t_eph::bad ||
778 ephLast->checkState() == t_eph::outdated ||
779 ephLast->checkState() == t_eph::unhealthy) {
780#ifdef BNC_DEBUG_CMB
781 emit newMessage("bncComb: ephLast not ok (checkState: " + ephLast->checkStateToString().toLatin1() + ") for " + prnStr.mid(0,3).toLatin1(), true);
782#endif
783 delete _newCorr; _newCorr = 0;
784 continue;
785 }
786 else {
787 if (ephLast->IOD() == _newCorr->_iod) {
788 _newCorr->_eph = ephLast;
789 }
790 else if (ephPrev &&
791 !outDatedBcep(ephPrev, currentTime) && // if not updated in storage
792 ephPrev->checkState() == t_eph::ok && // received
793 ephPrev->IOD() == _newCorr->_iod) {
794 _newCorr->_eph = ephPrev;
795 switchToLastEph(ephLast, _newCorr);
796 }
797 else {
798#ifdef BNC_DEBUG_CMB
799 emit newMessage("bncComb: eph not found for " + prnStr.mid(0,3).toLatin1() +
800 QString(" with IOD %1").arg(_newCorr->_iod).toLatin1(), true);
801#endif
802 delete _newCorr; _newCorr = 0;
803 continue;
804 }
805 }
806
807 // Check satellite code biases
808 // ----------------------------
809 if (_satCodeBiases.contains(acName)) {
810 QMap<t_prn, t_satCodeBias>& storage = _satCodeBiases[acName];
811 if (storage.contains(clkCorr._prn)) {
812 _newCorr->_satCodeBias = storage[clkCorr._prn];
813 QMap<t_frequency::type, double> codeBiasesRefSig;
814 for (unsigned ii = 1; ii < cmbRefSig::cIF; ii++) {
815 t_frequency::type frqType = cmbRefSig::toFreq(sys, static_cast<cmbRefSig::type>(ii));
816 char frqNum = t_frequency::toString(frqType)[1];
817 char attrib = cmbRefSig::toAttrib(sys, static_cast<cmbRefSig::type>(ii));
818 QString rnxType2ch = QString("%1%2").arg(frqNum).arg(attrib);
819 for (unsigned ii = 0; ii < _newCorr->_satCodeBias._bias.size(); ii++) {
820 const t_frqCodeBias& bias = _newCorr->_satCodeBias._bias[ii];
821 if (rnxType2ch.toStdString() == bias._rnxType2ch) {
822 codeBiasesRefSig[frqType] = bias._value;
823 }
824 }
825 }
826 map<t_frequency::type, double> codeCoeff;
827 double channel = double(_newCorr->_eph->slotNum());
828 cmbRefSig::coeff(sys, cmbRefSig::cIF, channel, codeCoeff);
829 map<t_frequency::type, double>::const_iterator it;
830 for (it = codeCoeff.begin(); it != codeCoeff.end(); it++) {
831 t_frequency::type frqType = it->first;
832 double codeCoeff = it->second;
833 _newCorr->_satCodeBiasIF += codeCoeff * codeBiasesRefSig[frqType];
834 }
835 _newCorr->_satCodeBias._bias.clear();
836 }
837 }
838
839 // Check satellite phase biases
840 // ----------------------------
841 if (_satPhaseBiases.contains(acName)) {
842 QMap<t_prn, t_satPhaseBias>& storage = _satPhaseBiases[acName];
843 if (storage.contains(clkCorr._prn)) {
844 // Yaw angle
845 t_satPhaseBias& pb = storage[clkCorr._prn];
846 double dt = _newCorr->_time - pb._time;
847 if (pb._updateInt) {
848 dt -= (0.5 * ssrUpdateInt[pb._updateInt]);
849 }
850 _newCorr->_satYawAngle = pb._yaw + pb._yawRate * dt;
851
852 // _lambdaIF
853 map<t_frequency::type, double> codeCoeff;
854 double channel = double(_newCorr->_eph->slotNum());
855 cmbRefSig::coeff(sys, cmbRefSig::cIF, channel, codeCoeff);
856 map<t_frequency::type, double>::const_iterator it;
857 for (it = codeCoeff.begin(); it != codeCoeff.end(); it++) {
858 t_frequency::type frqType = it->first;
859 double codeCoeff = it->second;
860 _newCorr->_lambdaIF += codeCoeff * t_CST::c / t_CST::freq(frqType, channel);
861 }
862 }
863 }
864
865 // Store correction into the buffer
866 // --------------------------------
867 QVector<cmbCorr*>& corrs = _buffer[sys].corrs;
868 QVectorIterator<cmbCorr*> itCorr(corrs);
869 bool available = false;
870 while (itCorr.hasNext()) {
871 cmbCorr* corr = itCorr.next();
872 QString prnStr = corr->_prn;
873 QString acName = corr->_acName;
874 if (_newCorr->_acName == acName &&
875 _newCorr->_prn == prnStr) {
876 available = true;
877 }
878 }
879
880 if (!available) {
881 corrs.push_back(_newCorr); _newCorr = 0;
882 }
883 else {
884 delete _newCorr; _newCorr = 0;
885 continue;
886 }
887 }
888
889 // Process Systems of this Epoch
890 // ------------------------------
891 QMapIterator<char, unsigned> itSys(_cmbSysPrn);
892 while (itSys.hasNext()) {
893 itSys.next();
894 char sys = itSys.key();
895 _log.clear();
896 QTextStream out(&_log, QIODevice::WriteOnly);
897 processSystem(epoTime, sys, out);
898 _buffer.remove(sys);
899 if (_logFile) {
900 _logFile->write(epoTime.gpsw(),epoTime.gpssec(), QString(_log));
901 }
902 }
903}
904
905void bncComb::processSystem(bncTime epoTime, char sys, QTextStream& out) {
906
907 out << "\n"
908 << epoTime.datestr().c_str() << " "
909 << epoTime.timestr().c_str() << " "
910 << "Combination: " << sys << "\n"
911 << "--------------------------------------" << "\n";
912
913 // Observation Statistics
914 // ----------------------
915 bool masterPresent = false;
916 QListIterator<cmbAC*> icAC(_ACs);
917 while (icAC.hasNext()) {
918 cmbAC* AC = icAC.next();
919 AC->numObs[sys] = 0;
920 QVectorIterator<cmbCorr*> itCorr(corrs(sys));
921 while (itCorr.hasNext()) {
922 cmbCorr* corr = itCorr.next();
923 if (corr->_acName == AC->name) {
924 AC->numObs[sys] += 1;
925 if (AC->name == _masterOrbitAC[sys]) {
926 masterPresent = true;
927 }
928 }
929 }
930 out << epoTime.datestr().c_str() << " "
931 << epoTime.timestr().c_str() << " "
932 << "Sat Num " << sys << " "
933 << AC->name.toLatin1().data() << ": " << AC->numObs[sys] << "\n";
934 }
935
936 // If Master not present, switch to another one
937 // --------------------------------------------
938 const unsigned switchMasterAfterGap = 1;
939 if (masterPresent) {
940 _masterMissingEpochs[sys] = 0;
941 }
942 else {
943 ++_masterMissingEpochs[sys];
944 if (_masterMissingEpochs[sys] < switchMasterAfterGap) {
945 out << "Missing Master, Epoch skipped" << "\n";
946 _buffer.remove(sys);
947 emit newMessage(_log, false);
948 return;
949 }
950 else {
951 _masterMissingEpochs[sys] = 0;
952 QListIterator<cmbAC*> icAC(_ACs);
953 while (icAC.hasNext()) {
954 cmbAC* AC = icAC.next();
955 if (AC->numObs[sys] > 0) {
956 out << epoTime.datestr().c_str() << " "
957 << epoTime.timestr().c_str() << " "
958 << "Switching Master " << sys << " "
959 << _masterOrbitAC[sys].toLatin1().data() << " --> "
960 << AC->name.toLatin1().data() << "\n";
961 _masterOrbitAC[sys] = AC->name;
962 _masterIsAPC[sys] = AC->isAPC;
963 break;
964 }
965 }
966 }
967 }
968
969 QMap<QString, cmbCorr*> masterCorr;
970
971 // Perform the actual Combination using selected Method
972 // ----------------------------------------------------
973 t_irc irc;
974 ColumnVector dx;
975 if (_method == filter) {
976 irc = processEpoch_filter(epoTime, sys, out, masterCorr, dx);
977 }
978 else {
979 irc = processEpoch_singleEpoch(epoTime, sys, out, masterCorr, dx);
980 }
981
982 // Update Parameter Values, Print Results
983 // --------------------------------------
984 if (irc == success) {
985 for (int iPar = 1; iPar <= _params[sys].size(); iPar++) {
986 cmbParam* pp = _params[sys][iPar-1];
987 pp->xx += dx(iPar);
988 if (pp->type == cmbParam::clkSat) {
989 if (masterCorr.find(pp->prn) != masterCorr.end()) {
990 // set clock result
991 masterCorr[pp->prn]->_dClkResult = pp->xx / t_CST::c;
992 // Add Code Biases from SINEX File
993 if (_bsx) {
994 map<t_frequency::type, double> codeCoeff;
995 double channel = double(masterCorr[pp->prn]->_eph->slotNum());
996 cmbRefSig::coeff(sys, cmbRefSig::cIF, channel, codeCoeff);
997 t_frequency::type fType1 = cmbRefSig::toFreq(sys, cmbRefSig::c1);
998 t_frequency::type fType2 = cmbRefSig::toFreq(sys, cmbRefSig::c2);
999 _bsx->determineSsrSatCodeBiases(pp->prn.mid(0,3), codeCoeff[fType1], codeCoeff[fType2], masterCorr[pp->prn]->_satCodeBias);
1000 }
1001 }
1002 }
1003 out << epoTime.datestr().c_str() << " "
1004 << epoTime.timestr().c_str() << " ";
1005 out.setRealNumberNotation(QTextStream::FixedNotation);
1006 out.setFieldWidth(8);
1007 out.setRealNumberPrecision(4);
1008 out << pp->toString(sys) << " "
1009 << pp->xx << " +- " << sqrt(_QQ[sys](pp->index,pp->index)) << "\n";
1010 out.setFieldWidth(0);
1011 out.flush();
1012 }
1013 printResults(epoTime, out, masterCorr);
1014 dumpResults(epoTime, masterCorr);
1015 }
1016}
1017
1018// Process Epoch - Filter Method
1019////////////////////////////////////////////////////////////////////////////
1020t_irc bncComb::processEpoch_filter(bncTime epoTime, char sys, QTextStream& out,
1021 QMap<QString, cmbCorr*>& masterCorr,
1022 ColumnVector& dx) {
1023
1024 // Prediction Step
1025 // ---------------
1026 int nPar = _params[sys].size();
1027 ColumnVector x0(nPar);
1028 for (int iPar = 1; iPar <= nPar; iPar++) {
1029 cmbParam* pp = _params[sys][iPar-1];
1030 if (pp->epoSpec) {
1031 pp->xx = 0.0;
1032 _QQ[sys].Row(iPar) = 0.0;
1033 _QQ[sys].Column(iPar) = 0.0;
1034 _QQ[sys](iPar,iPar) = pp->sig0 * pp->sig0;
1035 }
1036 else {
1037 _QQ[sys](iPar,iPar) += pp->sigP * pp->sigP;
1038 }
1039 x0(iPar) = pp->xx;
1040 }
1041
1042 // Check Satellite Positions for Outliers
1043 // --------------------------------------
1044 if (checkOrbits(epoTime, sys, out, masterCorr) != success) {
1045 return failure;
1046 }
1047
1048 // Update and outlier detection loop
1049 // ---------------------------------
1050 SymmetricMatrix QQ_sav = _QQ[sys];
1051 while (_running) {
1052
1053 Matrix AA;
1054 ColumnVector ll;
1055 DiagonalMatrix PP;
1056
1057 if (createAmat(sys, AA, ll, PP, x0) != success) {
1058 return failure;
1059 }
1060
1061 dx.ReSize(nPar); dx = 0.0;
1062 kalman(AA, ll, PP, _QQ[sys], dx);
1063
1064 ColumnVector vv = ll - AA * dx;
1065
1066 int maxResIndex;
1067 double maxRes = vv.maximum_absolute_value1(maxResIndex);
1068 out.setRealNumberNotation(QTextStream::FixedNotation);
1069 out.setRealNumberPrecision(3);
1070 out << epoTime.datestr().c_str() << " " << epoTime.timestr().c_str()
1071 << " Maximum Residuum " << maxRes << ' '
1072 << corrs(sys)[maxResIndex-1]->_acName << ' ' << corrs(sys)[maxResIndex-1]->_prn.mid(0,3);
1073 if (maxRes > _MAX_RES) {
1074 for (int iPar = 1; iPar <= _params[sys].size(); iPar++) {
1075 cmbParam* pp = _params[sys][iPar-1];
1076 if (pp->type == cmbParam::offACSat &&
1077 pp->AC == corrs(sys)[maxResIndex-1]->_acName &&
1078 pp->prn == corrs(sys)[maxResIndex-1]->_prn.mid(0,3)) {
1079 QQ_sav.Row(iPar) = 0.0;
1080 QQ_sav.Column(iPar) = 0.0;
1081 QQ_sav(iPar,iPar) = pp->sig0 * pp->sig0;
1082 }
1083 }
1084
1085 out << " Outlier" << "\n";
1086 _QQ[sys] = QQ_sav;
1087 delete corrs(sys)[maxResIndex-1];
1088 corrs(sys).remove(maxResIndex-1);
1089 }
1090 else {
1091 out << " OK" << "\n";
1092 out.setRealNumberNotation(QTextStream::FixedNotation);
1093 out.setRealNumberPrecision(4);
1094 for (int ii = 0; ii < corrs(sys).size(); ii++) {
1095 const cmbCorr* corr = corrs(sys)[ii];
1096 out << epoTime.datestr().c_str() << ' '
1097 << epoTime.timestr().c_str() << " "
1098 << corr->_acName << ' ' << corr->_prn.mid(0,3);
1099 out.setFieldWidth(10);
1100 out << " res = " << vv[ii] << "\n";
1101 out.setFieldWidth(0);
1102 }
1103 break;
1104 }
1105 }
1106
1107 return success;
1108}
1109
1110// Print results
1111////////////////////////////////////////////////////////////////////////////
1112void bncComb::printResults(bncTime epoTime, QTextStream& out,
1113 const QMap<QString, cmbCorr*>& masterCorr) {
1114
1115 QMapIterator<QString, cmbCorr*> it(masterCorr);
1116 while (it.hasNext()) {
1117 it.next();
1118 cmbCorr* corr = it.value();
1119 const t_eph* eph = corr->_eph;
1120 if (eph) {
1121 ColumnVector xc(6);
1122 ColumnVector vv(3);
1123 if (eph->getCrd(epoTime, xc, vv, false) != success) {
1124 continue;
1125 }
1126 out << epoTime.datestr().c_str() << " "
1127 << epoTime.timestr().c_str() << " ";
1128 out.setFieldWidth(3);
1129 out << "Full Clock " << corr->_prn.mid(0,3) << " " << corr->_iod << " ";
1130 out.setFieldWidth(14);
1131 out << (xc(4) + corr->_dClkResult) * t_CST::c << "\n";
1132 out.setFieldWidth(0);
1133 out.flush();
1134 }
1135 else {
1136 out << "bncComb::printResults bug" << "\n";
1137 }
1138 }
1139}
1140
1141// Send results to RTNet Decoder and directly to PPP Client
1142////////////////////////////////////////////////////////////////////////////
1143void bncComb::dumpResults(bncTime epoTime, QMap<QString, cmbCorr*>& masterCorr) {
1144
1145 QList<t_orbCorr> orbCorrections;
1146 QList<t_clkCorr> clkCorrections;
1147 QList<t_satCodeBias> satCodeBiasList;
1148 QList<t_satPhaseBias> satPhaseBiasList;
1149
1150 unsigned year, month, day, hour, minute;
1151 double sec;
1152 epoTime.civil_date(year, month, day);
1153 epoTime.civil_time(hour, minute, sec);
1154
1155 QString outLines = QString().asprintf("* %4d %2d %2d %d %d %12.8f\n",
1156 year, month, day, hour, minute, sec);
1157
1158 QMutableMapIterator<QString, cmbCorr*> it(masterCorr);
1159 while (it.hasNext()) {
1160 it.next();
1161 cmbCorr* corr = it.value();
1162
1163 // ORBIT
1164 t_orbCorr orbCorr(corr->_orbCorr);
1165 orbCorr._staID = "INTERNAL";
1166 orbCorrections.push_back(orbCorr);
1167
1168 // CLOCK
1169 t_clkCorr clkCorr(corr->_clkCorr);
1170 clkCorr._staID = "INTERNAL";
1171 clkCorr._dClk = corr->_dClkResult;
1172 clkCorr._dotDClk = 0.0;
1173 clkCorr._dotDotDClk = 0.0;
1174 clkCorrections.push_back(clkCorr);
1175
1176 ColumnVector xc(6);
1177 ColumnVector vv(3);
1178 corr->_eph->setClkCorr(dynamic_cast<const t_clkCorr*>(&clkCorr));
1179 corr->_eph->setOrbCorr(dynamic_cast<const t_orbCorr*>(&orbCorr));
1180 if (corr->_eph->getCrd(epoTime, xc, vv, true) != success) {
1181 delete corr;
1182 it.remove();
1183 continue;
1184 }
1185
1186 // Correction Phase Center --> CoM
1187 // -------------------------------
1188 ColumnVector dx(3); dx = 0.0;
1189 ColumnVector apc(3); apc = 0.0;
1190 ColumnVector com(3); com = 0.0;
1191 bool masterIsAPC = true;
1192 if (_antex) {
1193 double Mjd = epoTime.mjd() + epoTime.daysec()/86400.0;
1194 char sys = corr->_eph->prn().system();
1195 masterIsAPC = _masterIsAPC[sys];
1196 if (_antex->satCoMcorrection(corr->_prn, Mjd, xc.Rows(1,3), dx) != success) {
1197 dx = 0;
1198 _log += "antenna not found " + corr->_prn.mid(0,3).toLatin1() + '\n';
1199 }
1200 }
1201 if (masterIsAPC) {
1202 apc(1) = xc(1);
1203 apc(2) = xc(2);
1204 apc(3) = xc(3);
1205 com(1) = xc(1)-dx(1);
1206 com(2) = xc(2)-dx(2);
1207 com(3) = xc(3)-dx(3);
1208 }
1209 else {
1210 com(1) = xc(1);
1211 com(2) = xc(2);
1212 com(3) = xc(3);
1213 apc(1) = xc(1)+dx(1);
1214 apc(2) = xc(2)+dx(2);
1215 apc(3) = xc(3)+dx(3);
1216 }
1217
1218 outLines += corr->_prn.mid(0,3);
1219 QString hlp = QString().asprintf(
1220 " APC 3 %15.4f %15.4f %15.4f"
1221 " Clk 1 %15.4f"
1222 " Vel 3 %15.4f %15.4f %15.4f"
1223 " CoM 3 %15.4f %15.4f %15.4f"
1224 " YawAngle %1.4f",
1225 apc(1), apc(2), apc(3),
1226 xc(4) * t_CST::c,
1227 vv(1), vv(2), vv(3),
1228 com(1), com(2), com(3),
1229 corr->_satYawAngle);
1230 outLines += hlp;
1231 hlp.clear();
1232
1233 // CODE BIASES
1234 if (corr->_satCodeBias._bias.size()) {
1235 t_satCodeBias satCodeBias(corr->_satCodeBias);
1236 satCodeBias._staID = "INTERNAL";
1237 satCodeBiasList.push_back(satCodeBias);
1238 hlp = QString().asprintf(" CodeBias %2lu", satCodeBias._bias.size());
1239 outLines += hlp;
1240 hlp.clear();
1241 for (unsigned ii = 0; ii < satCodeBias._bias.size(); ii++) {
1242 const t_frqCodeBias& frqCodeBias = satCodeBias._bias[ii];
1243 if (!frqCodeBias._rnxType2ch.empty()) {
1244 hlp = QString().asprintf(" %s%10.6f", frqCodeBias._rnxType2ch.c_str(), frqCodeBias._value);
1245 outLines += hlp;
1246 hlp.clear();
1247 }
1248 }
1249 }
1250
1251 outLines += "\n";
1252 delete corr;
1253 it.remove();
1254 }
1255
1256 outLines += "EOE\n"; // End Of Epoch flag
1257 //cout << outLines.toStdString();
1258
1259 vector<string> errmsg;
1260 _rtnetDecoder->Decode(outLines.toLatin1().data(), outLines.length(), errmsg);
1261
1262 // Send new Corrections to PPP etc.
1263 // --------------------------------
1264 if (orbCorrections.size() > 0 && clkCorrections.size() > 0) {
1265 emit newOrbCorrections(orbCorrections);
1266 emit newClkCorrections(clkCorrections);
1267 }
1268 if (satCodeBiasList.size()) {
1269 emit newCodeBiases(satCodeBiasList);
1270 }
1271}
1272
1273// Create First Design Matrix and Vector of Measurements
1274////////////////////////////////////////////////////////////////////////////
1275t_irc bncComb::createAmat(char sys, Matrix& AA, ColumnVector& ll, DiagonalMatrix& PP, const ColumnVector& x0) {
1276
1277 unsigned nPar = _params[sys].size();
1278 unsigned nObs = corrs(sys).size();
1279
1280 if (nObs == 0) {
1281 return failure;
1282 }
1283
1284 int maxSat = _cmbSysPrn[sys];
1285
1286 const int nCon = (_method == filter) ? 1 + maxSat : 0;
1287
1288 AA.ReSize(nObs+nCon, nPar); AA = 0.0;
1289 ll.ReSize(nObs+nCon); ll = 0.0;
1290 PP.ReSize(nObs+nCon); PP = 1.0 / (sigObs * sigObs);
1291
1292 int iObs = 0;
1293 QVectorIterator<cmbCorr*> itCorr(corrs(sys));
1294 while (itCorr.hasNext()) {
1295 cmbCorr* corr = itCorr.next();
1296 QString prn = corr->_prn;
1297
1298 ++iObs;
1299
1300 for (int iPar = 1; iPar <= _params[sys].size(); iPar++) {
1301 cmbParam* pp = _params[sys][iPar-1];
1302 AA(iObs, iPar) = pp->partial(sys, corr->_acName, prn);
1303 }
1304 // Consistency corrections to keep the combined clock consistent to masterOrbit
1305 // ----------------------------------------------------------------------------
1306 double dC_orb = dotproduct(corr->_orbCorr._xr, corr->_diffRao) / corr->_orbCorr._xr.NormFrobenius();
1307 double dC_att = corr->_diffYaw / (2 * M_PI);
1308 dC_att *= corr->_lambdaIF;
1309
1310 ll(iObs) = (corr->_clkCorr._dClk * t_CST::c - corr->_satCodeBiasIF + dC_orb + dC_att) - DotProduct(AA.Row(iObs), x0);
1311
1312 PP(iObs, iObs) *= 1.0 / (corr->_weightFactor * corr->_weightFactor);
1313 }
1314
1315 // Regularization
1316 // --------------
1317 if (_method == filter) {
1318 const double Ph = 1.e6;
1319 PP(nObs+1) = Ph;
1320 for (int iPar = 1; iPar <= _params[sys].size(); iPar++) {
1321 cmbParam* pp = _params[sys][iPar-1];
1322 if ( AA.Column(iPar).maximum_absolute_value() > 0.0 &&
1323 pp->type == cmbParam::clkSat ) {
1324 AA(nObs+1, iPar) = 1.0;
1325 }
1326 }
1327// if (sys == 'R') {
1328// return success;
1329// }
1330 int iCond = 1;
1331 // GNSS
1332 for (unsigned iGnss = 1; iGnss <= _cmbSysPrn[sys]; iGnss++) {
1333 int flag = t_corrSSR::getSsrNavTypeFlag(sys, iGnss);
1334 QString prn = QString("%1%2_%3").arg(sys).arg(iGnss, 2, 10, QChar('0')).arg(flag);
1335 ++iCond;
1336 PP(nObs+iCond) = Ph;
1337 for (int iPar = 1; iPar <= _params[sys].size(); iPar++) {
1338 cmbParam* pp = _params[sys][iPar-1];
1339 if ( pp &&
1340 AA.Column(iPar).maximum_absolute_value() > 0.0 &&
1341 pp->type == cmbParam::offACSat &&
1342 pp->prn == prn) {
1343 AA(nObs+iCond, iPar) = 1.0;
1344 }
1345 }
1346 }
1347 }
1348
1349 return success;
1350}
1351
1352// Process Epoch - Single-Epoch Method
1353////////////////////////////////////////////////////////////////////////////
1354t_irc bncComb::processEpoch_singleEpoch(bncTime epoTime, char sys, QTextStream& out,
1355 QMap<QString, cmbCorr*>& masterCorr,
1356 ColumnVector& dx) {
1357
1358 // Check Satellite Positions for Outliers
1359 // --------------------------------------
1360 if (checkOrbits(epoTime, sys, out, masterCorr) != success) {
1361 return failure;
1362 }
1363
1364 // Outlier Detection Loop
1365 // ----------------------
1366 while (_running) {
1367 // Count Number of Observations per Satellite and per AC
1368 // -----------------------------------------------------
1369 QMap<QString, int> numObsPrn;
1370 QMap<QString, int> numObsAC;
1371 QVectorIterator<cmbCorr*> itCorr(corrs(sys));
1372 while (itCorr.hasNext()) {
1373 cmbCorr* corr = itCorr.next();
1374 QString prn = corr->_prn;
1375 QString AC = corr->_acName;
1376 if (numObsPrn.find(prn) == numObsPrn.end()) {
1377 numObsPrn[prn] = 1;
1378 }
1379 else {
1380 numObsPrn[prn] += 1;
1381 }
1382 if (numObsAC.find(AC) == numObsAC.end()) {
1383 numObsAC[AC] = 1;
1384 }
1385 else {
1386 numObsAC[AC] += 1;
1387 }
1388 }
1389
1390 // Clean-Up the Parameters
1391 // -----------------------
1392 for (int iPar = 1; iPar <= _params[sys].size(); iPar++) {
1393 delete _params[sys][iPar-1];
1394 }
1395 _params[sys].clear();
1396
1397 // Set new Parameters
1398 // ------------------
1399 int nextPar = 0;
1400
1401 QMapIterator<QString, int> itAC(numObsAC);
1402 while (itAC.hasNext()) {
1403 itAC.next();
1404 const QString& AC = itAC.key();
1405 int numObs = itAC.value();
1406 if (AC != _masterOrbitAC[sys] && numObs > 0) {
1407 _params[sys].push_back(new cmbParam(cmbParam::offACgnss, ++nextPar, AC, ""));
1408 }
1409 }
1410
1411 QMapIterator<QString, int> itPrn(numObsPrn);
1412 while (itPrn.hasNext()) {
1413 itPrn.next();
1414 const QString& prn = itPrn.key();
1415 int numObs = itPrn.value();
1416 if (numObs > 0) {
1417 _params[sys].push_back(new cmbParam(cmbParam::clkSat, ++nextPar, "", prn));
1418 }
1419 }
1420
1421 int nPar = _params[sys].size();
1422 ColumnVector x0(nPar);
1423 x0 = 0.0;
1424
1425 // Create First-Design Matrix
1426 // --------------------------
1427 Matrix AA;
1428 ColumnVector ll;
1429 DiagonalMatrix PP;
1430 if (createAmat(sys, AA, ll, PP, x0) != success) {
1431 return failure;
1432 }
1433
1434 ColumnVector vv;
1435 try {
1436 Matrix ATP = AA.t() * PP;
1437 SymmetricMatrix NN; NN << ATP * AA;
1438 ColumnVector bb = ATP * ll;
1439 _QQ[sys] = NN.i();
1440 dx = _QQ[sys] * bb;
1441 vv = ll - AA * dx;
1442 }
1443 catch (Exception& exc) {
1444 out << exc.what() << "\n";
1445 return failure;
1446 }
1447
1448 int maxResIndex;
1449 double maxRes = vv.maximum_absolute_value1(maxResIndex);
1450 out.setRealNumberNotation(QTextStream::FixedNotation);
1451 out.setRealNumberPrecision(3);
1452 out << epoTime.datestr().c_str() << " " << epoTime.timestr().c_str()
1453 << " Maximum Residuum " << maxRes << ' '
1454 << corrs(sys)[maxResIndex-1]->_acName << ' ' << corrs(sys)[maxResIndex-1]->_prn.mid(0,3);
1455
1456 if (maxRes > _MAX_RES) {
1457 out << " Outlier" << "\n";
1458 delete corrs(sys)[maxResIndex-1];
1459 corrs(sys).remove(maxResIndex-1);
1460 }
1461 else {
1462 out << " OK" << "\n";
1463 out.setRealNumberNotation(QTextStream::FixedNotation);
1464 out.setRealNumberPrecision(3);
1465 for (int ii = 0; ii < vv.Nrows(); ii++) {
1466 const cmbCorr* corr = corrs(sys)[ii];
1467 out << epoTime.datestr().c_str() << ' '
1468 << epoTime.timestr().c_str() << " "
1469 << corr->_acName << ' ' << corr->_prn.mid(0,3);
1470 out.setFieldWidth(6);
1471 out << " res = " << vv[ii] << "\n";
1472 out.setFieldWidth(0);
1473 }
1474 return success;
1475 }
1476
1477 }
1478
1479 return failure;
1480}
1481
1482// Check Satellite Positions for Outliers
1483////////////////////////////////////////////////////////////////////////////
1484t_irc bncComb::checkOrbits(bncTime epoTime, char sys, QTextStream& out, QMap<QString, cmbCorr*>& masterCorr) {
1485
1486 // Switch to last ephemeris (if possible)
1487 // --------------------------------------
1488 QMutableVectorIterator<cmbCorr*> im(corrs(sys));
1489 while (im.hasNext()) {
1490 cmbCorr* corr = im.next();
1491 QString prn = corr->_prn;
1492
1493 t_eph* ephLast = _ephUser.ephLast(prn);
1494 t_eph* ephPrev = _ephUser.ephPrev(prn);
1495
1496 if (ephLast == 0) {
1497 out << "checkOrbit: missing eph (not found) " << corr->_prn.mid(0,3) << "\n";
1498 delete corr;
1499 im.remove();
1500 }
1501 else if (corr->_eph == 0) {
1502 out << "checkOrbit: missing eph (zero) " << corr->_prn.mid(0,3) << "\n";
1503 delete corr;
1504 im.remove();
1505 }
1506 else {
1507 if ( corr->_eph == ephLast ||
1508 corr->_eph == ephPrev ) {
1509 switchToLastEph(ephLast, corr);
1510 }
1511 else {
1512 out << "checkOrbit: missing eph (deleted) " << corr->_prn.mid(0,3) << "\n";
1513 delete corr;
1514 im.remove();
1515 }
1516 }
1517 }
1518
1519 while (_running) {
1520
1521 // Compute Mean Corrections for all Satellites
1522 // -------------------------------------------
1523 QMap<QString, int> numCorr;
1524 QMap<QString, ColumnVector> meanRao;
1525 QVectorIterator<cmbCorr*> it(corrs(sys));
1526 while (it.hasNext()) {
1527 cmbCorr* corr = it.next();
1528 QString prn = corr->_prn;
1529 if (meanRao.find(prn) == meanRao.end()) {
1530 meanRao[prn].ReSize(4);
1531 meanRao[prn].Rows(1,3) = corr->_orbCorr._xr;
1532 meanRao[prn](4) = 1;
1533 }
1534 else {
1535 meanRao[prn].Rows(1,3) += corr->_orbCorr._xr;
1536 meanRao[prn](4) += 1;
1537 }
1538 if (numCorr.find(prn) == numCorr.end()) {
1539 numCorr[prn] = 1;
1540 }
1541 else {
1542 numCorr[prn] += 1;
1543 }
1544 }
1545
1546 // Compute Differences wrt. Mean, find Maximum
1547 // -------------------------------------------
1548 QMap<QString, cmbCorr*> maxDiff;
1549 it.toFront();
1550 while (it.hasNext()) {
1551 cmbCorr* corr = it.next();
1552 QString prn = corr->_prn;
1553 if (meanRao[prn](4) != 0) {
1554 meanRao[prn] /= meanRao[prn](4);
1555 meanRao[prn](4) = 0;
1556 }
1557 corr->_diffRao = corr->_orbCorr._xr - meanRao[prn].Rows(1,3);
1558
1559 if (maxDiff.find(prn) == maxDiff.end()) {
1560 maxDiff[prn] = corr;
1561 }
1562 else {
1563 double normMax = maxDiff[prn]->_diffRao.NormFrobenius();
1564 double norm = corr->_diffRao.NormFrobenius();
1565 if (norm > normMax) {
1566 maxDiff[prn] = corr;
1567 }
1568 }
1569 }
1570
1571 if (_ACs.size() == 1) {
1572 QVectorIterator<cmbCorr*> it(corrs(sys));
1573 while (it.hasNext()) {
1574 cmbCorr* corr = it.next();
1575 QString prn = corr->_prn;
1576 if (corr->_acName == _masterOrbitAC[sys] &&
1577 masterCorr.find(prn) == masterCorr.end()) {
1578 masterCorr[prn] = new cmbCorr(*corr);
1579 masterCorr[prn]->_diffRao = 0.0;
1580 masterCorr[prn]->_diffYaw = 0.0;
1581 }
1582 }
1583 break;
1584 }
1585
1586 // Remove Outliers
1587 // ---------------
1588 bool removed = false;
1589 QMutableVectorIterator<cmbCorr*> im(corrs(sys));
1590 while (im.hasNext()) {
1591 cmbCorr* corr = im.next();
1592 QString prn = corr->_prn;
1593 if (numCorr[prn] < 2) {
1594 delete corr;
1595 im.remove();
1596 }
1597 else if (corr == maxDiff[prn]) {
1598 double norm = corr->_diffRao.NormFrobenius();
1599 double facSys = 1.0;
1600 if (sys == 'C' && prn.mid(1,2).toInt() <= 16) { // BDS-2
1601 facSys *= 5.0;
1602 }
1603 if (norm > (_MAX_DISPLACEMENT * facSys)) {
1604 out << epoTime.datestr().c_str() << " "
1605 << epoTime.timestr().c_str() << " "
1606 << "Orbit Outlier: "
1607 << corr->_acName.toLatin1().data() << " "
1608 << prn.mid(0,3).toLatin1().data() << " "
1609 << corr->_iod << " "
1610 << norm << "\n";
1611 delete corr;
1612 im.remove();
1613 removed = true;
1614 }
1615 }
1616 }
1617 // Set master orbit
1618 // -----------------------------------------
1619 if (!removed) {
1620 QVectorIterator<cmbCorr*> it(corrs(sys));
1621 while (it.hasNext()) {
1622 cmbCorr* corr = it.next();
1623 QString prn = corr->_prn;
1624 if (corr->_acName == _masterOrbitAC[sys] &&
1625 masterCorr.find(prn) == masterCorr.end()) {
1626 masterCorr[prn] = new cmbCorr(*corr);
1627 }
1628 }
1629 // Remove satellites that are not in masterOrbit
1630 // and compute differences wrt. masterOrbit
1631 // ----------------------------------------------
1632 QMutableVectorIterator<cmbCorr*> im(corrs(sys));
1633 while (im.hasNext()) {
1634 cmbCorr* corr = im.next();
1635 QString prn = corr->_prn;
1636 if (masterCorr.find(prn) == masterCorr.end()) {
1637 delete corr;
1638 im.remove();
1639 }
1640 else {
1641 corr->_diffRao = corr->_orbCorr._xr - masterCorr[prn]->_orbCorr._xr;
1642 corr->_diffYaw = corr->_satYawAngle - masterCorr[prn]->_satYawAngle;
1643 }
1644 }
1645 break;
1646 }
1647 }
1648
1649 return success;
1650}
1651
1652//
1653////////////////////////////////////////////////////////////////////////////
1654void bncComb::slotProviderIDChanged(QString mountPoint) {
1655 QMutexLocker locker(&_mutex);
1656
1657 QTextStream out(&_log, QIODevice::WriteOnly);
1658
1659 // Find the AC Name
1660 // ----------------
1661 QString acName;
1662 QListIterator<cmbAC*> icAC(_ACs);
1663 while (icAC.hasNext()) {
1664 cmbAC *AC = icAC.next();
1665 if (AC->mountPoint == mountPoint) {
1666 acName = AC->name;
1667 out << "Provider ID changed: AC " << AC->name.toLatin1().data() << " "
1668 << _resTime.datestr().c_str() << " " << _resTime.timestr().c_str()
1669 << "\n";
1670 break;
1671 }
1672 }
1673 if (acName.isEmpty()) {
1674 return;
1675 }
1676 QMapIterator<char, unsigned> itSys(_cmbSysPrn);
1677 while (itSys.hasNext()) {
1678 itSys.next();
1679 char sys = itSys.key();
1680 // Remove all corrections of the corresponding AC
1681 // ----------------------------------------------
1682 QVector<cmbCorr*> &corrVec = _buffer[sys].corrs;
1683 QMutableVectorIterator<cmbCorr*> it(corrVec);
1684 while (it.hasNext()) {
1685 cmbCorr *corr = it.next();
1686 if (acName == corr->_acName) {
1687 delete corr;
1688 it.remove();
1689 }
1690 }
1691 // Reset Satellite Offsets
1692 // -----------------------
1693 if (_method == filter) {
1694 for (int iPar = 1; iPar <= _params[sys].size(); iPar++) {
1695 cmbParam *pp = _params[sys][iPar - 1];
1696 if (pp->AC == acName && pp->type == cmbParam::offACSat) {
1697 pp->xx = 0.0;
1698 _QQ[sys].Row(iPar) = 0.0;
1699 _QQ[sys].Column(iPar) = 0.0;
1700 _QQ[sys](iPar, iPar) = pp->sig0 * pp->sig0;
1701 }
1702 }
1703 }
1704 }
1705}
1706
1707//
1708////////////////////////////////////////////////////////////////////////////
1709bool bncComb::excludeSat(const t_prn& prn, const QStringList excludeSats) const {
1710 QStringListIterator it(excludeSats);
1711 while (it.hasNext()) {
1712 string prnStr = it.next().toLatin1().data();
1713 if (prnStr == prn.toString() || // prn
1714 prnStr == prn.toString().substr(0,1)) { // sys
1715 return true;
1716 }
1717 }
1718 return false;
1719}
Note: See TracBrowser for help on using the repository browser.