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

Last change on this file since 8137 was 8065, checked in by stuerze, 8 years ago

logfile entry added

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