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

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