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

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