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

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