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

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