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

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