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

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