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

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