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

Last change on this file since 3474 was 3474, checked in by mervart, 13 years ago
File size: 23.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;
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() == "Filter") {
161 _method = filter;
162 }
163 else {
164 _method = singleEpoch;
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 if (_method == filter) {
441 irc = processEpoch_filter(out, resCorr);
442 }
443 else {
444 irc = processEpoch_singleEpoch(out, resCorr);
445 }
446
447 // Print Results
448 // -------------
449 if (irc == success) {
450 printResults(out, resCorr);
451 dumpResults(resCorr);
452 }
453
454 // Delete Data, emit Message
455 // -------------------------
456 _buffer.remove(_resTime);
457 emit newMessage(_log, false);
458}
459
460// Process Epoch - Filter Method
461////////////////////////////////////////////////////////////////////////////
462t_irc bncComb::processEpoch_filter(QTextStream& out,
463 QMap<QString, t_corr*>& resCorr) {
464
465 // Prediction Step
466 // ---------------
467 int nPar = _params.size();
468 ColumnVector x0(nPar);
469 for (int iPar = 1; iPar <= _params.size(); iPar++) {
470 cmbParam* pp = _params[iPar-1];
471 if (pp->epoSpec) {
472 pp->xx = 0.0;
473 _QQ.Row(iPar) = 0.0;
474 _QQ.Column(iPar) = 0.0;
475 _QQ(iPar,iPar) = pp->sig0 * pp->sig0;
476 }
477 else {
478 _QQ(iPar,iPar) += pp->sigP * pp->sigP;
479 }
480 x0(iPar) = pp->xx;
481 }
482
483 SymmetricMatrix QQ_sav = _QQ;
484
485 ColumnVector dx;
486
487 // Update and outlier detection loop
488 // ---------------------------------
489 while (true) {
490
491 Matrix AA;
492 ColumnVector ll;
493 DiagonalMatrix PP;
494
495 if (createAmat(AA, ll, PP, x0, resCorr) != success) {
496 return failure;
497 }
498
499 bncModel::kalman(AA, ll, PP, _QQ, dx);
500 ColumnVector vv = ll - AA * dx;
501
502 int maxResIndex;
503 double maxRes = vv.maximum_absolute_value1(maxResIndex);
504 out.setRealNumberNotation(QTextStream::FixedNotation);
505 out.setRealNumberPrecision(3);
506 out << _resTime.datestr().c_str() << " " << _resTime.timestr().c_str()
507 << " Maximum Residuum " << maxRes << ' '
508 << corrs()[maxResIndex-1]->acName << ' ' << corrs()[maxResIndex-1]->prn;
509
510 if (maxRes > _MAXRES) {
511 for (int iPar = 1; iPar <= _params.size(); iPar++) {
512 cmbParam* pp = _params[iPar-1];
513 if (pp->type == cmbParam::offACSat &&
514 pp->AC == corrs()[maxResIndex-1]->acName &&
515 pp->prn == corrs()[maxResIndex-1]->prn) {
516 QQ_sav.Row(iPar) = 0.0;
517 QQ_sav.Column(iPar) = 0.0;
518 QQ_sav(iPar,iPar) = pp->sig0 * pp->sig0;
519 }
520 }
521
522 out << " Outlier" << endl;
523 _QQ = QQ_sav;
524 corrs().remove(maxResIndex-1);
525 }
526 else {
527 out << " OK" << endl;
528 break;
529 }
530 }
531
532 // Update Parameter Values
533 // -----------------------
534 for (int iPar = 1; iPar <= _params.size(); iPar++) {
535 cmbParam* pp = _params[iPar-1];
536 pp->xx += dx(iPar);
537 if (pp->type == cmbParam::clkSat) {
538 if (resCorr.find(pp->prn) != resCorr.end()) {
539 resCorr[pp->prn]->dClk = pp->xx / t_CST::c;
540 }
541 }
542 out << _resTime.datestr().c_str() << " "
543 << _resTime.timestr().c_str() << " ";
544 out.setRealNumberNotation(QTextStream::FixedNotation);
545 out.setFieldWidth(8);
546 out.setRealNumberPrecision(4);
547 out << pp->toString() << " "
548 << pp->xx << " +- " << sqrt(_QQ(pp->index,pp->index)) << endl;
549 out.setFieldWidth(0);
550 }
551
552 return success;
553}
554
555// Print results
556////////////////////////////////////////////////////////////////////////////
557void bncComb::printResults(QTextStream& out,
558 const QMap<QString, t_corr*>& resCorr) {
559
560 QMapIterator<QString, t_corr*> it(resCorr);
561 while (it.hasNext()) {
562 it.next();
563 t_corr* corr = it.value();
564 const t_eph* eph = corr->eph;
565 if (eph) {
566 double xx, yy, zz, cc;
567 eph->position(_resTime.gpsw(), _resTime.gpssec(), xx, yy, zz, cc);
568
569 out << _resTime.datestr().c_str() << " "
570 << _resTime.timestr().c_str() << " ";
571 out.setFieldWidth(3);
572 out << "Full Clock " << corr->prn << " " << corr->iod << " ";
573 out.setFieldWidth(14);
574 out << (cc + corr->dClk) * t_CST::c << endl;
575 out.setFieldWidth(0);
576 }
577 else {
578 out << "bncComb::printResuls bug" << endl;
579 }
580 }
581}
582
583// Send results to RTNet Decoder and directly to PPP Client
584////////////////////////////////////////////////////////////////////////////
585void bncComb::dumpResults(const QMap<QString, t_corr*>& resCorr) {
586
587 ostringstream out; out.setf(std::ios::fixed);
588 QStringList corrLines;
589
590 unsigned year, month, day, hour, minute;
591 double sec;
592 _resTime.civil_date(year, month, day);
593 _resTime.civil_time(hour, minute, sec);
594
595 out << "* "
596 << setw(4) << year << " "
597 << setw(2) << month << " "
598 << setw(2) << day << " "
599 << setw(2) << hour << " "
600 << setw(2) << minute << " "
601 << setw(12) << setprecision(8) << sec << " "
602 << endl;
603
604 QMapIterator<QString, t_corr*> it(resCorr);
605 while (it.hasNext()) {
606 it.next();
607 t_corr* corr = it.value();
608
609 double dT = 60.0;
610
611 for (int iTime = 1; iTime <= 2; iTime++) {
612
613 bncTime time12 = (iTime == 1) ? _resTime : _resTime + dT;
614
615 ColumnVector xc(4);
616 ColumnVector vv(3);
617 corr->eph->position(time12.gpsw(), time12.gpssec(),
618 xc.data(), vv.data());
619 bncPPPclient::applyCorr(time12, corr, xc, vv);
620
621 // Relativistic Correction
622 // -----------------------
623 double relCorr = - 2.0 * DotProduct(xc.Rows(1,3),vv) / t_CST::c / t_CST::c;
624 xc(4) -= relCorr;
625
626 // Code Biases
627 // -----------
628 double dcbP1C1 = 0.0;
629 double dcbP1P2 = 0.0;
630
631 // Correction Phase Center --> CoM
632 // -------------------------------
633 ColumnVector dx(3); dx = 0.0;
634 if (_antex) {
635 double Mjd = time12.mjd() + time12.daysec()/86400.0;
636 if (_antex->satCoMcorrection(corr->prn, Mjd, xc.Rows(1,3), dx) == success) {
637 xc(1) -= dx(1);
638 xc(2) -= dx(2);
639 xc(3) -= dx(3);
640 }
641 else {
642 cout << "antenna not found" << endl;
643 }
644 }
645
646 if (iTime == 1) {
647 out << 'P' << corr->prn.toAscii().data()
648 << setw(14) << setprecision(6) << xc(1) / 1000.0
649 << setw(14) << setprecision(6) << xc(2) / 1000.0
650 << setw(14) << setprecision(6) << xc(3) / 1000.0
651 << setw(14) << setprecision(6) << xc(4) * 1e6
652 << setw(14) << setprecision(6) << relCorr * 1e6
653 << setw(8) << setprecision(3) << dx(1)
654 << setw(8) << setprecision(3) << dx(2)
655 << setw(8) << setprecision(3) << dx(3)
656 << setw(8) << setprecision(3) << dcbP1C1
657 << setw(8) << setprecision(3) << dcbP1P2
658 << setw(6) << setprecision(1) << dT;
659
660 QString line;
661 int messageType = COTYPE_GPSCOMBINED;
662 int updateInt = 0;
663 line.sprintf("%d %d %d %.1f %s"
664 " %3d"
665 " %8.3f %8.3f %8.3f %8.3f"
666 " %10.5f %10.5f %10.5f %10.5f"
667 " %10.5f %10.5f %10.5f %10.5f INTERNAL",
668 messageType, updateInt, time12.gpsw(), time12.gpssec(),
669 corr->prn.toAscii().data(),
670 corr->iod,
671 corr->dClk * t_CST::c,
672 corr->rao[0],
673 corr->rao[1],
674 corr->rao[2],
675 corr->dotDClk * t_CST::c,
676 corr->dotRao[0],
677 corr->dotRao[1],
678 corr->dotRao[2],
679 corr->dotDotDClk * t_CST::c,
680 corr->dotDotRao[0],
681 corr->dotDotRao[1],
682 corr->dotDotRao[2]);
683 corrLines << line;
684 }
685 else {
686 out << setw(14) << setprecision(6) << xc(1) / 1000.0
687 << setw(14) << setprecision(6) << xc(2) / 1000.0
688 << setw(14) << setprecision(6) << xc(3) / 1000.0 << endl;
689 }
690 }
691
692 delete corr;
693 }
694 out << "EOE" << endl; // End Of Epoch flag
695
696 if (!_rtnetDecoder) {
697 _rtnetDecoder = new bncRtnetDecoder();
698 }
699
700 vector<string> errmsg;
701 _rtnetDecoder->Decode((char*) out.str().data(), out.str().size(), errmsg);
702
703 // Optionally send new Corrections to PPP
704 // --------------------------------------
705 bncApp* app = (bncApp*) qApp;
706 if (app->_bncPPPclient) {
707 app->_bncPPPclient->slotNewCorrections(corrLines);
708 }
709}
710
711// Create First Design Matrix and Vector of Measurements
712////////////////////////////////////////////////////////////////////////////
713t_irc bncComb::createAmat(Matrix& AA, ColumnVector& ll, DiagonalMatrix& PP,
714 const ColumnVector& x0,
715 QMap<QString, t_corr*>& resCorr) {
716
717 unsigned nPar = _params.size();
718 unsigned nObs = corrs().size();
719
720 if (nObs == 0) {
721 return failure;
722 }
723
724 const int nCon = (_method == filter) ? 1 + MAXPRN_GPS : 0;
725
726 AA.ReSize(nObs+nCon, nPar); AA = 0.0;
727 ll.ReSize(nObs+nCon); ll = 0.0;
728 PP.ReSize(nObs+nCon); PP = 1.0 / (sigObs * sigObs);
729
730 int iObs = 0;
731
732 QVectorIterator<cmbCorr*> itCorr(corrs());
733 while (itCorr.hasNext()) {
734 cmbCorr* corr = itCorr.next();
735 QString prn = corr->prn;
736 switchToLastEph(_eph[prn]->last, corr);
737 ++iObs;
738
739 if (_method == filter) {
740 if (corr->acName == _masterOrbitAC && resCorr.find(prn) == resCorr.end()) {
741 resCorr[prn] = new t_corr(*corr);
742 }
743 }
744
745 for (int iPar = 1; iPar <= _params.size(); iPar++) {
746 cmbParam* pp = _params[iPar-1];
747 AA(iObs, iPar) = pp->partial(corr->acName, prn);
748 }
749
750 ll(iObs) = corr->dClk * t_CST::c - DotProduct(AA.Row(iObs), x0);
751 }
752
753 // Regularization
754 // --------------
755 if (_method == filter) {
756 const double Ph = 1.e6;
757 PP(nObs+1) = Ph;
758 for (int iPar = 1; iPar <= _params.size(); iPar++) {
759 cmbParam* pp = _params[iPar-1];
760 if ( AA.Column(iPar).maximum_absolute_value() > 0.0 &&
761 pp->type == cmbParam::clkSat ) {
762 AA(nObs+1, iPar) = 1.0;
763 }
764 }
765 int iCond = 1;
766 for (int iGps = 1; iGps <= MAXPRN_GPS; iGps++) {
767 QString prn = QString("G%1").arg(iGps, 2, 10, QChar('0'));
768 ++iCond;
769 PP(nObs+iCond) = Ph;
770 for (int iPar = 1; iPar <= _params.size(); iPar++) {
771 cmbParam* pp = _params[iPar-1];
772 if ( AA.Column(iPar).maximum_absolute_value() > 0.0 &&
773 pp->type == cmbParam::offACSat &&
774 pp->prn == prn) {
775 AA(nObs+iCond, iPar) = 1.0;
776 }
777 }
778 }
779 }
780
781 return success;
782}
783
784// Process Epoch - Single-Epoch Method
785////////////////////////////////////////////////////////////////////////////
786t_irc bncComb::processEpoch_singleEpoch(QTextStream& out,
787 QMap<QString, t_corr*>& resCorr) {
788
789 // Initialize resCorr
790 // ------------------
791 QVectorIterator<cmbCorr*> itCorr(corrs());
792 while (itCorr.hasNext()) {
793 cmbCorr* corr = itCorr.next();
794 QString prn = corr->prn;
795 switchToLastEph(_eph[prn]->last, corr);
796 if (corr->acName == _masterOrbitAC && resCorr.find(prn) == resCorr.end()) {
797 resCorr[prn] = new t_corr(*corr);
798 }
799 }
800
801 // Count Number of Observations per Satellite and per AC
802 // -----------------------------------------------------
803 QMap<QString, int> numObsPrn;
804 QMap<QString, int> numObsAC;
805 QMapIterator<QString, t_corr*> itRes(resCorr);
806 while (itRes.hasNext()) {
807 itRes.next();
808 const QString& prnRes = itRes.key();
809 QVectorIterator<cmbCorr*> itCorr(corrs());
810 while (itCorr.hasNext()) {
811 cmbCorr* corr = itCorr.next();
812 QString prn = corr->prn;
813 QString AC = corr->acName;
814 if (prn == prnRes) {
815 if (numObsPrn.find(prn) == numObsPrn.end()) {
816 numObsPrn[prn] = 1;
817 }
818 else {
819 numObsPrn[prn] += 1;
820 }
821 if (numObsAC.find(prn) == numObsAC.end()) {
822 numObsAC[AC] = 1;
823 }
824 else {
825 numObsAC[AC] += 1;
826 }
827 }
828 }
829 }
830
831 // Clean-Up the Paramters
832 // ----------------------
833 for (int iPar = 1; iPar <= _params.size(); iPar++) {
834 delete _params[iPar-1];
835 }
836 _params.clear();
837
838 // Set new Parameters
839 // ------------------
840 int nextPar = 0;
841
842 QMapIterator<QString, int> itAC(numObsAC);
843 while (itAC.hasNext()) {
844 itAC.next();
845 const QString& AC = itAC.key();
846 int numObs = itAC.value();
847 if (numObs > 0) {
848 _params.push_back(new cmbParam(cmbParam::offAC, ++nextPar, AC, ""));
849 }
850 }
851
852 QMapIterator<QString, int> itPrn(numObsPrn);
853 while (itPrn.hasNext()) {
854 itPrn.next();
855 const QString& prn = itPrn.key();
856 int numObs = itPrn.value();
857 if (numObs > 0) {
858 _params.push_back(new cmbParam(cmbParam::clkSat, ++nextPar, "", prn));
859 }
860 }
861
862 int nPar = _params.size();
863 ColumnVector x0(nPar);
864 x0 = 0.0;
865
866 // Create First-Design Matrix
867 // --------------------------
868 Matrix AA;
869 ColumnVector ll;
870 DiagonalMatrix PP;
871 if (createAmat(AA, ll, PP, x0, resCorr) != success) {
872 return failure;
873 }
874
875 SymmetricMatrix NN; NN << AA.t() * PP * AA;
876 ColumnVector bb = AA.t() * PP * ll;
877 SymmetricMatrix QQ = NN.i();
878
879 ColumnVector xx = QQ * bb;
880
881 cout << xx.t() << endl;
882
883 return success;
884}
Note: See TracBrowser for help on using the repository browser.