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

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