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

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