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

Last change on this file since 8938 was 8901, checked in by stuerze, 5 years ago

upgrade to newmat11 library

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