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

Last change on this file since 9671 was 9671, checked in by stuerze, 3 months ago

minor changes to allow combination sampling iterval of 5 seconds

File size: 35.9 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 = 5;
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::bad ||
432 ephLast->checkState() == t_eph::outdated ||
433 ephLast->checkState() == t_eph::unhealthy) {
434 emit newMessage("bncComb: eph not ok for " + prn.mid(0,3).toLatin1(), true);
435 delete newCorr;
436 continue;
437 }
438 else {
439 if (ephLast->IOD() == newCorr->_iod) {
440 newCorr->_eph = ephLast;
441 }
442 else if (ephPrev && ephPrev->checkState() == t_eph::ok &&
443 ephPrev->IOD() == newCorr->_iod) {
444 newCorr->_eph = ephPrev;
445 switchToLastEph(ephLast, newCorr);
446 }
447 else {
448 emit newMessage("bncComb: eph not found for " + prn.mid(0,3).toAscii() +
449 QString(" with IOD %1").arg(newCorr->_iod).toAscii(), true);
450 delete newCorr;
451 continue;
452 }
453 }
454
455 // Store correction into the buffer
456 // --------------------------------
457 QVector<cmbCorr*>& corrs = _buffer[newCorr->_time].corrs;
458 corrs.push_back(newCorr);
459 }
460
461 // Process previous Epoch(s)
462 // -------------------------
463 const double outWait = 1.0 * _cmbSampl;
464 QListIterator<bncTime> itTime(_buffer.keys());
465 while (itTime.hasNext()) {
466 bncTime epoTime = itTime.next();
467 if (epoTime < lastTime - outWait) {
468 _resTime = epoTime;
469 processEpoch();
470 }
471 }
472}
473
474// Change the correction so that it refers to last received ephemeris
475////////////////////////////////////////////////////////////////////////////
476void bncComb::switchToLastEph(t_eph* lastEph, cmbCorr* corr) {
477
478 if (corr->_eph == lastEph) {
479 return;
480 }
481
482 ColumnVector oldXC(6);
483 ColumnVector oldVV(3);
484 if (corr->_eph->getCrd(corr->_time, oldXC, oldVV, false) != success) {
485 return;
486 }
487
488 ColumnVector newXC(6);
489 ColumnVector newVV(3);
490 if (lastEph->getCrd(corr->_time, newXC, newVV, false) != success) {
491 return;
492 }
493
494 ColumnVector dX = newXC.Rows(1,3) - oldXC.Rows(1,3);
495 ColumnVector dV = newVV - oldVV;
496 double dC = newXC(4) - oldXC(4);
497
498 ColumnVector dRAO(3);
499 XYZ_to_RSW(newXC.Rows(1,3), newVV, dX, dRAO);
500
501 ColumnVector dDotRAO(3);
502 XYZ_to_RSW(newXC.Rows(1,3), newVV, dV, dDotRAO);
503
504 QString msg = "switch corr " + corr->_prn.mid(0,3)
505 + QString(" %1 -> %2 %3").arg(corr->_iod,3).arg(lastEph->IOD(),3).arg(dC*t_CST::c, 8, 'f', 4);
506
507 emit newMessage(msg.toAscii(), false);
508
509 corr->_iod = lastEph->IOD();
510 corr->_eph = lastEph;
511
512 corr->_orbCorr._xr += dRAO;
513 corr->_orbCorr._dotXr += dDotRAO;
514 corr->_clkCorr._dClk -= dC;
515}
516
517// Process Epoch
518////////////////////////////////////////////////////////////////////////////
519void bncComb::processEpoch() {
520
521 _log.clear();
522
523 QTextStream out(&_log, QIODevice::WriteOnly);
524
525 out << endl << "Combination:" << endl
526 << "------------------------------" << endl;
527
528 // Observation Statistics
529 // ----------------------
530 bool masterPresent = false;
531 QListIterator<cmbAC*> icAC(_ACs);
532 while (icAC.hasNext()) {
533 cmbAC* AC = icAC.next();
534 AC->numObs = 0;
535 QVectorIterator<cmbCorr*> itCorr(corrs());
536 while (itCorr.hasNext()) {
537 cmbCorr* corr = itCorr.next();
538 if (corr->_acName == AC->name) {
539 AC->numObs += 1;
540 if (AC->name == _masterOrbitAC) {
541 masterPresent = true;
542 }
543 }
544 }
545 out << AC->name.toAscii().data() << ": " << AC->numObs << endl;
546 }
547
548 // If Master not present, switch to another one
549 // --------------------------------------------
550 const unsigned switchMasterAfterGap = 1;
551 if (masterPresent) {
552 _masterMissingEpochs = 0;
553 }
554 else {
555 ++_masterMissingEpochs;
556 if (_masterMissingEpochs < switchMasterAfterGap) {
557 out << "Missing Master, Epoch skipped" << endl;
558 _buffer.remove(_resTime);
559 emit newMessage(_log, false);
560 return;
561 }
562 else {
563 _masterMissingEpochs = 0;
564 QListIterator<cmbAC*> icAC(_ACs);
565 while (icAC.hasNext()) {
566 cmbAC* AC = icAC.next();
567 if (AC->numObs > 0) {
568 out << "Switching Master AC "
569 << _masterOrbitAC.toAscii().data() << " --> "
570 << AC->name.toAscii().data() << " "
571 << _resTime.datestr().c_str() << " "
572 << _resTime.timestr().c_str() << endl;
573 _masterOrbitAC = AC->name;
574 break;
575 }
576 }
577 }
578 }
579
580 QMap<QString, cmbCorr*> resCorr;
581
582 // Perform the actual Combination using selected Method
583 // ----------------------------------------------------
584 t_irc irc;
585 ColumnVector dx;
586 if (_method == filter) {
587 irc = processEpoch_filter(out, resCorr, dx);
588 }
589 else {
590 irc = processEpoch_singleEpoch(out, resCorr, dx);
591 }
592
593 // Update Parameter Values, Print Results
594 // --------------------------------------
595 if (irc == success) {
596 for (int iPar = 1; iPar <= _params.size(); iPar++) {
597 cmbParam* pp = _params[iPar-1];
598 pp->xx += dx(iPar);
599 if (pp->type == cmbParam::clkSat) {
600 if (resCorr.find(pp->prn) != resCorr.end()) {
601 resCorr[pp->prn]->_dClkResult = pp->xx / t_CST::c;
602 }
603 }
604 out << _resTime.datestr().c_str() << " "
605 << _resTime.timestr().c_str() << " ";
606 out.setRealNumberNotation(QTextStream::FixedNotation);
607 out.setFieldWidth(8);
608 out.setRealNumberPrecision(4);
609 out << pp->toString() << " "
610 << pp->xx << " +- " << sqrt(_QQ(pp->index,pp->index)) << endl;
611 out.setFieldWidth(0);
612 }
613 printResults(out, resCorr);
614 dumpResults(resCorr);
615 }
616
617 // Delete Data, emit Message
618 // -------------------------
619 _buffer.remove(_resTime);
620 emit newMessage(_log, false);
621}
622
623// Process Epoch - Filter Method
624////////////////////////////////////////////////////////////////////////////
625t_irc bncComb::processEpoch_filter(QTextStream& out,
626 QMap<QString, cmbCorr*>& resCorr,
627 ColumnVector& dx) {
628
629 // Prediction Step
630 // ---------------
631 int nPar = _params.size();
632 ColumnVector x0(nPar);
633 for (int iPar = 1; iPar <= _params.size(); iPar++) {
634 cmbParam* pp = _params[iPar-1];
635 if (pp->epoSpec) {
636 pp->xx = 0.0;
637 _QQ.Row(iPar) = 0.0;
638 _QQ.Column(iPar) = 0.0;
639 _QQ(iPar,iPar) = pp->sig0 * pp->sig0;
640 }
641 else {
642 _QQ(iPar,iPar) += pp->sigP * pp->sigP;
643 }
644 x0(iPar) = pp->xx;
645 }
646
647 // Check Satellite Positions for Outliers
648 // --------------------------------------
649 if (checkOrbits(out) != success) {
650 return failure;
651 }
652
653 // Update and outlier detection loop
654 // ---------------------------------
655 SymmetricMatrix QQ_sav = _QQ;
656 while (true) {
657
658 Matrix AA;
659 ColumnVector ll;
660 DiagonalMatrix PP;
661
662 if (createAmat(AA, ll, PP, x0, resCorr) != success) {
663 return failure;
664 }
665
666 dx.ReSize(nPar); dx = 0.0;
667 kalman(AA, ll, PP, _QQ, dx);
668
669 ColumnVector vv = ll - AA * dx;
670
671 int maxResIndex;
672 double maxRes = vv.maximum_absolute_value1(maxResIndex);
673 out.setRealNumberNotation(QTextStream::FixedNotation);
674 out.setRealNumberPrecision(3);
675 out << _resTime.datestr().c_str() << " " << _resTime.timestr().c_str()
676 << " Maximum Residuum " << maxRes << ' '
677 << corrs()[maxResIndex-1]->_acName << ' ' << corrs()[maxResIndex-1]->_prn.mid(0,3);
678 if (maxRes > _MAXRES) {
679 for (int iPar = 1; iPar <= _params.size(); iPar++) {
680 cmbParam* pp = _params[iPar-1];
681 if (pp->type == cmbParam::offACSat &&
682 pp->AC == corrs()[maxResIndex-1]->_acName &&
683 pp->prn == corrs()[maxResIndex-1]->_prn.mid(0,3)) {
684 QQ_sav.Row(iPar) = 0.0;
685 QQ_sav.Column(iPar) = 0.0;
686 QQ_sav(iPar,iPar) = pp->sig0 * pp->sig0;
687 }
688 }
689
690 out << " Outlier" << endl;
691 _QQ = QQ_sav;
692 corrs().remove(maxResIndex-1);
693 }
694 else {
695 out << " OK" << endl;
696 out.setRealNumberNotation(QTextStream::FixedNotation);
697 out.setRealNumberPrecision(4);
698 for (int ii = 0; ii < corrs().size(); ii++) {
699 const cmbCorr* corr = corrs()[ii];
700 out << _resTime.datestr().c_str() << ' '
701 << _resTime.timestr().c_str() << " "
702 << corr->_acName << ' ' << corr->_prn.mid(0,3);
703 out.setFieldWidth(10);
704 out << " res = " << vv[ii] << endl;
705 out.setFieldWidth(0);
706 }
707 break;
708 }
709 }
710
711 return success;
712}
713
714// Print results
715////////////////////////////////////////////////////////////////////////////
716void bncComb::printResults(QTextStream& out,
717 const QMap<QString, cmbCorr*>& resCorr) {
718
719 QMapIterator<QString, cmbCorr*> it(resCorr);
720 while (it.hasNext()) {
721 it.next();
722 cmbCorr* corr = it.value();
723 const t_eph* eph = corr->_eph;
724 if (eph) {
725 ColumnVector xc(6);
726 ColumnVector vv(3);
727 if (eph->getCrd(_resTime, xc, vv, false) != success) {
728 continue;
729 }
730 out << _resTime.datestr().c_str() << " "
731 << _resTime.timestr().c_str() << " ";
732 out.setFieldWidth(3);
733 out << "Full Clock " << corr->_prn.mid(0,3) << " " << corr->_iod << " ";
734 out.setFieldWidth(14);
735 out << (xc(4) + corr->_dClkResult) * t_CST::c << endl;
736 out.setFieldWidth(0);
737 }
738 else {
739 out << "bncComb::printResuls bug" << endl;
740 }
741 }
742}
743
744// Send results to RTNet Decoder and directly to PPP Client
745////////////////////////////////////////////////////////////////////////////
746void bncComb::dumpResults(const QMap<QString, cmbCorr*>& resCorr) {
747
748 QList<t_orbCorr> orbCorrections;
749 QList<t_clkCorr> clkCorrections;
750
751 QString outLines;
752 unsigned year, month, day, hour, minute;
753 double sec;
754 _resTime.civil_date(year, month, day);
755 _resTime.civil_time(hour, minute, sec);
756
757 outLines.sprintf("* %4d %2d %2d %d %d %12.8f\n",
758 year, month, day, hour, minute, sec);
759
760 QMapIterator<QString, cmbCorr*> it(resCorr);
761 while (it.hasNext()) {
762 it.next();
763 cmbCorr* corr = it.value();
764
765 t_orbCorr orbCorr(corr->_orbCorr);
766 orbCorr._staID = "INTERNAL";
767 orbCorrections.push_back(orbCorr);
768
769 t_clkCorr clkCorr(corr->_clkCorr);
770 clkCorr._staID = "INTERNAL";
771 clkCorr._dClk = corr->_dClkResult;
772 clkCorr._dotDClk = 0.0;
773 clkCorr._dotDotDClk = 0.0;
774 clkCorrections.push_back(clkCorr);
775
776 ColumnVector xc(6);
777 ColumnVector vv(3);
778 corr->_eph->setClkCorr(dynamic_cast<const t_clkCorr*>(&clkCorr));
779 corr->_eph->setOrbCorr(dynamic_cast<const t_orbCorr*>(&orbCorr));
780 if (corr->_eph->getCrd(_resTime, xc, vv, true) != success) {
781 delete corr;
782 continue;
783 }
784
785 // Correction Phase Center --> CoM
786 // -------------------------------
787 ColumnVector dx(3); dx = 0.0;
788 if (_antex) {
789 double Mjd = _resTime.mjd() + _resTime.daysec()/86400.0;
790 if (_antex->satCoMcorrection(corr->_prn, Mjd, xc.Rows(1,3), dx) != success) {
791 dx = 0;
792 _log += "antenna not found " + corr->_prn.mid(0,3).toAscii() + '\n';
793 }
794 }
795
796 outLines += corr->_prn.mid(0,3);
797 QString hlp;
798 hlp.sprintf(" APC 3 %15.4f %15.4f %15.4f"
799 " Clk 1 %15.4f"
800 " Vel 3 %15.4f %15.4f %15.4f"
801 " CoM 3 %15.4f %15.4f %15.4f\n",
802 xc(1), xc(2), xc(3),
803 xc(4) * t_CST::c,
804 vv(1), vv(2), vv(3),
805 xc(1)-dx(1), xc(2)-dx(2), xc(3)-dx(3));
806 outLines += hlp;
807
808 QString line;
809 int messageType = _ssrCorr->COTYPE_GPSCOMBINED;
810 int updateInt = 0;
811 line.sprintf("%d %d %d %.1f %s"
812 " %lu"
813 " %8.3f %8.3f %8.3f %8.3f"
814 " %10.5f %10.5f %10.5f %10.5f"
815 " %10.5f INTERNAL",
816 messageType, updateInt, _resTime.gpsw(), _resTime.gpssec(),
817 corr->_prn.mid(0,3).toAscii().data(),
818 corr->_iod,
819 corr->_dClkResult * t_CST::c,
820 corr->_orbCorr._xr[0],
821 corr->_orbCorr._xr[1],
822 corr->_orbCorr._xr[2],
823 0.0,
824 corr->_orbCorr._dotXr[0],
825 corr->_orbCorr._dotXr[1],
826 corr->_orbCorr._dotXr[2],
827 0.0);
828 delete corr;
829 }
830
831 outLines += "EOE\n"; // End Of Epoch flag
832
833 if (!_rtnetDecoder) {
834 _rtnetDecoder = new bncRtnetDecoder();
835 }
836
837 vector<string> errmsg;
838 _rtnetDecoder->Decode(outLines.toAscii().data(), outLines.length(), errmsg);
839
840 // Send new Corrections to PPP etc.
841 // --------------------------------
842 if (orbCorrections.size() > 0 && clkCorrections.size() > 0) {
843 emit newOrbCorrections(orbCorrections);
844 emit newClkCorrections(clkCorrections);
845 }
846}
847
848// Create First Design Matrix and Vector of Measurements
849////////////////////////////////////////////////////////////////////////////
850t_irc bncComb::createAmat(Matrix& AA, ColumnVector& ll, DiagonalMatrix& PP,
851 const ColumnVector& x0,
852 QMap<QString, cmbCorr*>& resCorr) {
853
854 unsigned nPar = _params.size();
855 unsigned nObs = corrs().size();
856
857 if (nObs == 0) {
858 return failure;
859 }
860
861 int maxSat = t_prn::MAXPRN_GPS;
862// if (_useGlonass) {
863// maxSat = t_prn::MAXPRN_GPS + t_prn::MAXPRN_GLONASS;
864// }
865
866 const int nCon = (_method == filter) ? 1 + maxSat : 0;
867
868 AA.ReSize(nObs+nCon, nPar); AA = 0.0;
869 ll.ReSize(nObs+nCon); ll = 0.0;
870 PP.ReSize(nObs+nCon); PP = 1.0 / (sigObs * sigObs);
871
872 int iObs = 0;
873 QVectorIterator<cmbCorr*> itCorr(corrs());
874 while (itCorr.hasNext()) {
875 cmbCorr* corr = itCorr.next();
876 QString prn = corr->_prn;
877
878 ++iObs;
879
880 if (corr->_acName == _masterOrbitAC && resCorr.find(prn) == resCorr.end()) {
881 resCorr[prn] = new cmbCorr(*corr);
882 }
883
884 for (int iPar = 1; iPar <= _params.size(); iPar++) {
885 cmbParam* pp = _params[iPar-1];
886 AA(iObs, iPar) = pp->partial(corr->_acName, prn);
887 }
888
889 ll(iObs) = corr->_clkCorr._dClk * t_CST::c - DotProduct(AA.Row(iObs), x0);
890 }
891
892 // Regularization
893 // --------------
894 if (_method == filter) {
895 const double Ph = 1.e6;
896 PP(nObs+1) = Ph;
897 for (int iPar = 1; iPar <= _params.size(); iPar++) {
898 cmbParam* pp = _params[iPar-1];
899 if ( AA.Column(iPar).maximum_absolute_value() > 0.0 &&
900 pp->type == cmbParam::clkSat ) {
901 AA(nObs+1, iPar) = 1.0;
902 }
903 }
904 int iCond = 1;
905 for (unsigned iGps = 1; iGps <= t_prn::MAXPRN_GPS; iGps++) {
906 QString prn = QString("G%1_0").arg(iGps, 2, 10, QChar('0'));
907 ++iCond;
908 PP(nObs+iCond) = Ph;
909 for (int iPar = 1; iPar <= _params.size(); iPar++) {
910 cmbParam* pp = _params[iPar-1];
911 if ( pp &&
912 AA.Column(iPar).maximum_absolute_value() > 0.0 &&
913 pp->type == cmbParam::offACSat &&
914 pp->prn == prn) {
915 AA(nObs+iCond, iPar) = 1.0;
916 }
917 }
918 }
919// if (_useGlonass) {
920// for (unsigned iGlo = 1; iGlo <= t_prn::MAXPRN_GLONASS; iGlo++) {
921// QString prn = QString("R%1_0").arg(iGlo, 2, 10, QChar('0'));
922// ++iCond;
923// PP(nObs+iCond) = Ph;
924// for (int iPar = 1; iPar <= _params.size(); iPar++) {
925// cmbParam* pp = _params[iPar-1];
926// if ( pp &&
927// AA.Column(iPar).maximum_absolute_value() > 0.0 &&
928// pp->type == cmbParam::offACSat &&
929// pp->prn == prn) {
930// AA(nObs+iCond, iPar) = 1.0;
931// }
932// }
933// }
934// }
935 }
936
937 return success;
938}
939
940// Process Epoch - Single-Epoch Method
941////////////////////////////////////////////////////////////////////////////
942t_irc bncComb::processEpoch_singleEpoch(QTextStream& out,
943 QMap<QString, cmbCorr*>& resCorr,
944 ColumnVector& dx) {
945
946 // Check Satellite Positions for Outliers
947 // --------------------------------------
948 if (checkOrbits(out) != success) {
949 return failure;
950 }
951
952 // Outlier Detection Loop
953 // ----------------------
954 while (true) {
955
956 // Remove Satellites that are not in Master
957 // ----------------------------------------
958 QMutableVectorIterator<cmbCorr*> it(corrs());
959 while (it.hasNext()) {
960 cmbCorr* corr = it.next();
961 QString prn = corr->_prn;
962 bool foundMaster = false;
963 QVectorIterator<cmbCorr*> itHlp(corrs());
964 while (itHlp.hasNext()) {
965 cmbCorr* corrHlp = itHlp.next();
966 QString prnHlp = corrHlp->_prn;
967 QString ACHlp = corrHlp->_acName;
968 if (ACHlp == _masterOrbitAC && prn == prnHlp) {
969 foundMaster = true;
970 break;
971 }
972 }
973 if (!foundMaster) {
974 delete corr;
975 it.remove();
976 }
977 }
978
979 // Count Number of Observations per Satellite and per AC
980 // -----------------------------------------------------
981 QMap<QString, int> numObsPrn;
982 QMap<QString, int> numObsAC;
983 QVectorIterator<cmbCorr*> itCorr(corrs());
984 while (itCorr.hasNext()) {
985 cmbCorr* corr = itCorr.next();
986 QString prn = corr->_prn;
987 QString AC = corr->_acName;
988 if (numObsPrn.find(prn) == numObsPrn.end()) {
989 numObsPrn[prn] = 1;
990 }
991 else {
992 numObsPrn[prn] += 1;
993 }
994 if (numObsAC.find(AC) == numObsAC.end()) {
995 numObsAC[AC] = 1;
996 }
997 else {
998 numObsAC[AC] += 1;
999 }
1000 }
1001
1002 // Clean-Up the Paramters
1003 // ----------------------
1004 for (int iPar = 1; iPar <= _params.size(); iPar++) {
1005 delete _params[iPar-1];
1006 }
1007 _params.clear();
1008
1009 // Set new Parameters
1010 // ------------------
1011 int nextPar = 0;
1012
1013 QMapIterator<QString, int> itAC(numObsAC);
1014 while (itAC.hasNext()) {
1015 itAC.next();
1016 const QString& AC = itAC.key();
1017 int numObs = itAC.value();
1018 if (AC != _masterOrbitAC && numObs > 0) {
1019 _params.push_back(new cmbParam(cmbParam::offACgps, ++nextPar, AC, ""));
1020 if (_useGlonass) {
1021 _params.push_back(new cmbParam(cmbParam::offACglo, ++nextPar, AC, ""));
1022 }
1023 }
1024 }
1025
1026 QMapIterator<QString, int> itPrn(numObsPrn);
1027 while (itPrn.hasNext()) {
1028 itPrn.next();
1029 const QString& prn = itPrn.key();
1030 int numObs = itPrn.value();
1031 if (numObs > 0) {
1032 _params.push_back(new cmbParam(cmbParam::clkSat, ++nextPar, "", prn));
1033 }
1034 }
1035
1036 int nPar = _params.size();
1037 ColumnVector x0(nPar);
1038 x0 = 0.0;
1039
1040 // Create First-Design Matrix
1041 // --------------------------
1042 Matrix AA;
1043 ColumnVector ll;
1044 DiagonalMatrix PP;
1045 if (createAmat(AA, ll, PP, x0, resCorr) != success) {
1046 return failure;
1047 }
1048
1049 ColumnVector vv;
1050 try {
1051 Matrix ATP = AA.t() * PP;
1052 SymmetricMatrix NN; NN << ATP * AA;
1053 ColumnVector bb = ATP * ll;
1054 _QQ = NN.i();
1055 dx = _QQ * bb;
1056 vv = ll - AA * dx;
1057 }
1058 catch (Exception& exc) {
1059 out << exc.what() << endl;
1060 return failure;
1061 }
1062
1063 int maxResIndex;
1064 double maxRes = vv.maximum_absolute_value1(maxResIndex);
1065 out.setRealNumberNotation(QTextStream::FixedNotation);
1066 out.setRealNumberPrecision(3);
1067 out << _resTime.datestr().c_str() << " " << _resTime.timestr().c_str()
1068 << " Maximum Residuum " << maxRes << ' '
1069 << corrs()[maxResIndex-1]->_acName << ' ' << corrs()[maxResIndex-1]->_prn.mid(0,3);
1070
1071 if (maxRes > _MAXRES) {
1072 out << " Outlier" << endl;
1073 delete corrs()[maxResIndex-1];
1074 corrs().remove(maxResIndex-1);
1075 }
1076 else {
1077 out << " OK" << endl;
1078 out.setRealNumberNotation(QTextStream::FixedNotation);
1079 out.setRealNumberPrecision(3);
1080 for (int ii = 0; ii < vv.Nrows(); ii++) {
1081 const cmbCorr* corr = corrs()[ii];
1082 out << _resTime.datestr().c_str() << ' '
1083 << _resTime.timestr().c_str() << " "
1084 << corr->_acName << ' ' << corr->_prn.mid(0,3);
1085 out.setFieldWidth(6);
1086 out << " res = " << vv[ii] << endl;
1087 out.setFieldWidth(0);
1088 }
1089 return success;
1090 }
1091
1092 }
1093
1094 return failure;
1095}
1096
1097// Check Satellite Positions for Outliers
1098////////////////////////////////////////////////////////////////////////////
1099t_irc bncComb::checkOrbits(QTextStream& out) {
1100
1101 const double MAX_DISPLACEMENT = 0.20;
1102
1103 // Switch to last ephemeris (if possible)
1104 // --------------------------------------
1105 QMutableVectorIterator<cmbCorr*> im(corrs());
1106 while (im.hasNext()) {
1107 cmbCorr* corr = im.next();
1108 QString prn = corr->_prn;
1109
1110 t_eph* ephLast = _ephUser.ephLast(prn);
1111 t_eph* ephPrev = _ephUser.ephPrev(prn);
1112
1113 if (ephLast == 0) {
1114 out << "checkOrbit: missing eph (not found) " << corr->_prn.mid(0,3) << endl;
1115 delete corr;
1116 im.remove();
1117 }
1118 else if (corr->_eph == 0) {
1119 out << "checkOrbit: missing eph (zero) " << corr->_prn.mid(0,3) << endl;
1120 delete corr;
1121 im.remove();
1122 }
1123 else {
1124 if ( corr->_eph == ephLast || corr->_eph == ephPrev ) {
1125 switchToLastEph(ephLast, corr);
1126 }
1127 else {
1128 out << "checkOrbit: missing eph (deleted) " << corr->_prn.mid(0,3) << endl;
1129 delete corr;
1130 im.remove();
1131 }
1132 }
1133 }
1134
1135 while (true) {
1136
1137 // Compute Mean Corrections for all Satellites
1138 // -------------------------------------------
1139 QMap<QString, int> numCorr;
1140 QMap<QString, ColumnVector> meanRao;
1141 QVectorIterator<cmbCorr*> it(corrs());
1142 while (it.hasNext()) {
1143 cmbCorr* corr = it.next();
1144 QString prn = corr->_prn;
1145 if (meanRao.find(prn) == meanRao.end()) {
1146 meanRao[prn].ReSize(4);
1147 meanRao[prn].Rows(1,3) = corr->_orbCorr._xr;
1148 meanRao[prn](4) = 1;
1149 }
1150 else {
1151 meanRao[prn].Rows(1,3) += corr->_orbCorr._xr;
1152 meanRao[prn](4) += 1;
1153 }
1154 if (numCorr.find(prn) == numCorr.end()) {
1155 numCorr[prn] = 1;
1156 }
1157 else {
1158 numCorr[prn] += 1;
1159 }
1160 }
1161
1162 // Compute Differences wrt Mean, find Maximum
1163 // ------------------------------------------
1164 QMap<QString, cmbCorr*> maxDiff;
1165 it.toFront();
1166 while (it.hasNext()) {
1167 cmbCorr* corr = it.next();
1168 QString prn = corr->_prn;
1169 if (meanRao[prn](4) != 0) {
1170 meanRao[prn] /= meanRao[prn](4);
1171 meanRao[prn](4) = 0;
1172 }
1173 corr->_diffRao = corr->_orbCorr._xr - meanRao[prn].Rows(1,3);
1174 if (maxDiff.find(prn) == maxDiff.end()) {
1175 maxDiff[prn] = corr;
1176 }
1177 else {
1178 double normMax = maxDiff[prn]->_diffRao.norm_Frobenius();
1179 double norm = corr->_diffRao.norm_Frobenius();
1180 if (norm > normMax) {
1181 maxDiff[prn] = corr;
1182 }
1183 }
1184 }
1185
1186 if (_ACs.size() == 1) {
1187 break;
1188 }
1189
1190 // Remove Outliers
1191 // ---------------
1192 bool removed = false;
1193 QMutableVectorIterator<cmbCorr*> im(corrs());
1194 while (im.hasNext()) {
1195 cmbCorr* corr = im.next();
1196 QString prn = corr->_prn;
1197 if (numCorr[prn] < 2) {
1198 delete corr;
1199 im.remove();
1200 }
1201 else if (corr == maxDiff[prn]) {
1202 double norm = corr->_diffRao.norm_Frobenius();
1203 if (norm > MAX_DISPLACEMENT) {
1204 out << _resTime.datestr().c_str() << " "
1205 << _resTime.timestr().c_str() << " "
1206 << "Orbit Outlier: "
1207 << corr->_acName.toAscii().data() << " "
1208 << prn.mid(0,3).toAscii().data() << " "
1209 << corr->_iod << " "
1210 << norm << endl;
1211 delete corr;
1212 im.remove();
1213 removed = true;
1214 }
1215 }
1216 }
1217
1218 if (!removed) {
1219 break;
1220 }
1221 }
1222
1223 return success;
1224}
1225
1226//
1227////////////////////////////////////////////////////////////////////////////
1228void bncComb::slotProviderIDChanged(QString mountPoint) {
1229 QMutexLocker locker(&_mutex);
1230
1231 QTextStream out(&_log, QIODevice::WriteOnly);
1232
1233 // Find the AC Name
1234 // ----------------
1235 QString acName;
1236 QListIterator<cmbAC*> icAC(_ACs);
1237 while (icAC.hasNext()) {
1238 cmbAC* AC = icAC.next();
1239 if (AC->mountPoint == mountPoint) {
1240 acName = AC->name;
1241 out << "Provider ID changed: AC " << AC->name.toAscii().data() << " "
1242 << _resTime.datestr().c_str() << " "
1243 << _resTime.timestr().c_str() << endl;
1244 break;
1245 }
1246 }
1247 if (acName.isEmpty()) {
1248 return;
1249 }
1250
1251 // Remove all corrections of the corresponding AC
1252 // ----------------------------------------------
1253 QListIterator<bncTime> itTime(_buffer.keys());
1254 while (itTime.hasNext()) {
1255 bncTime epoTime = itTime.next();
1256 QVector<cmbCorr*>& corrVec = _buffer[epoTime].corrs;
1257 QMutableVectorIterator<cmbCorr*> it(corrVec);
1258 while (it.hasNext()) {
1259 cmbCorr* corr = it.next();
1260 if (acName == corr->_acName) {
1261 delete corr;
1262 it.remove();
1263 }
1264 }
1265 }
1266
1267 // Reset Satellite Offsets
1268 // -----------------------
1269 if (_method == filter) {
1270 for (int iPar = 1; iPar <= _params.size(); iPar++) {
1271 cmbParam* pp = _params[iPar-1];
1272 if (pp->AC == acName && pp->type == cmbParam::offACSat) {
1273 pp->xx = 0.0;
1274 _QQ.Row(iPar) = 0.0;
1275 _QQ.Column(iPar) = 0.0;
1276 _QQ(iPar,iPar) = pp->sig0 * pp->sig0;
1277 }
1278 }
1279 }
1280}
Note: See TracBrowser for help on using the repository browser.