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

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