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

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