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

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