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

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