source: ntrip/branches/BNC_2.12/src/combination/bnccomb.cpp@ 9267

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

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

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