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

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