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

Last change on this file since 9261 was 9261, checked in by stuerze, 20 months ago

minor changes

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