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

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