source: ntrip/trunk/BNC/combination/bnccomb.cpp@ 3486

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