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

Last change on this file since 3049 was 3049, checked in by mervart, 13 years ago
File size: 16.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
20#include "bnccomb.h"
21#include "bncapp.h"
22#include "cmbcaster.h"
23#include "bncsettings.h"
24#include "bncmodel.h"
25#include "bncutils.h"
26#include "bncpppclient.h"
27#include "bnssp3.h"
28
29using namespace std;
30
31// Constructor
32////////////////////////////////////////////////////////////////////////////
33cmbParam::cmbParam(cmbParam::parType type_, int index_,
34 const QString& ac_, const QString& prn_,
35 double sig_0_, double sig_P_) {
36
37 type = type_;
38 index = index_;
39 AC = ac_;
40 prn = prn_;
41 sig_0 = sig_0_;
42 sig_P = sig_P_;
43 xx = 0.0;
44}
45
46// Destructor
47////////////////////////////////////////////////////////////////////////////
48cmbParam::~cmbParam() {
49}
50
51// Partial
52////////////////////////////////////////////////////////////////////////////
53double cmbParam::partial(const QString& AC_, t_corr* corr) {
54
55 if (type == AC_offset) {
56 if (AC == AC_) {
57 return 1.0;
58 }
59 }
60 else if (type == Sat_offset) {
61 if (AC == AC_ && prn == corr->prn) {
62 return 1.0;
63 }
64 }
65 else if (type == clk) {
66 if (prn == corr->prn) {
67 return 1.0;
68 }
69 }
70
71 return 0.0;
72}
73
74//
75////////////////////////////////////////////////////////////////////////////
76QString cmbParam::toString() const {
77
78 QString outStr;
79
80 if (type == AC_offset) {
81 outStr = "AC offset " + AC;
82 }
83 else if (type == Sat_offset) {
84 outStr = "Sat Offset " + AC + " " + prn;
85 }
86 else if (type == clk) {
87 outStr = "Clk Corr " + prn;
88 }
89
90 return outStr;
91}
92
93// Constructor
94////////////////////////////////////////////////////////////////////////////
95bncComb::bncComb() {
96
97 bncSettings settings;
98
99 QStringList combineStreams = settings.value("combineStreams").toStringList();
100
101 _masterAC = "BKG"; // TODO: make it an option
102
103 if (combineStreams.size() >= 2) {
104 QListIterator<QString> it(combineStreams);
105 while (it.hasNext()) {
106 QStringList hlp = it.next().split(" ");
107 cmbAC* newAC = new cmbAC();
108 newAC->mountPoint = hlp[0];
109 newAC->name = hlp[1];
110 newAC->weight = hlp[2].toDouble();
111
112 _ACs[newAC->mountPoint] = newAC;
113 }
114 }
115
116 _caster = new cmbCaster();
117 connect(this, SIGNAL(newMessage(QByteArray,bool)),
118 ((bncApp*)qApp), SLOT(slotMessage(const QByteArray,bool)));
119
120
121 // A Priori Sigmas (in Meters)
122 // ---------------------------
123 double sigAC_0 = 100.0;
124 double sigAC_P = 100.0;
125 double sigSat_0 = 100.0;
126 double sigSat_P = 0.0;
127 double sigClk_0 = 100.0;
128 double sigClk_P = 100.0;
129
130 // Initialize Parameters (model: Clk_Corr = AC_Offset + Sat_Offset + Clk)
131 // ----------------------------------------------------------------------
132 int nextPar = 0;
133 QMapIterator<QString, cmbAC*> it(_ACs);
134 while (it.hasNext()) {
135 it.next();
136 cmbAC* AC = it.value();
137 if (AC->name != _masterAC) {
138 _params.push_back(new cmbParam(cmbParam::AC_offset, ++nextPar,
139 AC->name, "", sigAC_0, sigAC_P));
140 for (int iGps = 1; iGps <= 32; iGps++) {
141 QString prn = QString("G%1").arg(iGps, 2, 10, QChar('0'));
142 _params.push_back(new cmbParam(cmbParam::Sat_offset, ++nextPar,
143 AC->name, prn, sigSat_0, sigSat_P));
144 }
145 }
146 }
147 for (int iGps = 1; iGps <= 32; iGps++) {
148 QString prn = QString("G%1").arg(iGps, 2, 10, QChar('0'));
149 _params.push_back(new cmbParam(cmbParam::clk, ++nextPar, "", prn,
150 sigClk_0, sigClk_P));
151 }
152
153 // Initialize Variance-Covariance Matrix
154 // -------------------------------------
155 _QQ.ReSize(_params.size());
156 _QQ = 0.0;
157 for (int iPar = 1; iPar <= _params.size(); iPar++) {
158 cmbParam* pp = _params[iPar-1];
159 _QQ(iPar,iPar) = pp->sig_0 * pp->sig_0;
160 }
161
162 // Output File (skeleton name)
163 // ---------------------------
164 QString path = settings.value("cmbOutPath").toString();
165 if (!path.isEmpty() && !_caster->mountpoint().isEmpty()) {
166 expandEnvVar(path);
167 if ( path.length() > 0 && path[path.length()-1] != QDir::separator() ) {
168 path += QDir::separator();
169 }
170 _outNameSkl = path + _caster->mountpoint();
171 }
172 _out = 0;
173
174 // SP3 writer
175 // ----------
176 if ( settings.value("cmbSP3Path").toString().isEmpty() ) {
177 _sp3 = 0;
178 }
179 else {
180 QString prep = "BNC";
181 QString ext = ".sp3";
182 QString path = settings.value("cmbSP3Path").toString();
183 QString interval = "";
184 int sampl = 0;
185 _sp3 = new bnsSP3(prep, ext, path, interval, sampl);
186 }
187
188 _append = Qt::CheckState(settings.value("rnxAppend").toInt()) == Qt::Checked;
189}
190
191// Destructor
192////////////////////////////////////////////////////////////////////////////
193bncComb::~bncComb() {
194 QMapIterator<QString, cmbAC*> it(_ACs);
195 while (it.hasNext()) {
196 it.next();
197 delete it.value();
198 }
199 delete _caster;
200 delete _out;
201 delete _sp3;
202}
203
204// Read and store one correction line
205////////////////////////////////////////////////////////////////////////////
206void bncComb::processCorrLine(const QString& staID, const QString& line) {
207 QMutexLocker locker(&_mutex);
208
209 // Find the relevant instance of cmbAC class
210 // -----------------------------------------
211 if (_ACs.find(staID) == _ACs.end()) {
212 return;
213 }
214 cmbAC* AC = _ACs[staID];
215
216 // Read the Correction
217 // -------------------
218 t_corr* newCorr = new t_corr();
219 if (!newCorr->readLine(line) == success) {
220 delete newCorr;
221 return;
222 }
223
224 // Reject delayed corrections
225 // --------------------------
226 if (_processedBeforeTime.valid() && newCorr->tt < _processedBeforeTime) {
227 delete newCorr;
228 return;
229 }
230
231 // Check the Ephemeris
232 //--------------------
233 if (_eph.find(newCorr->prn) == _eph.end()) {
234 delete newCorr;
235 return;
236 }
237 else {
238 t_eph* lastEph = _eph[newCorr->prn]->last;
239 t_eph* prevEph = _eph[newCorr->prn]->prev;
240 if (lastEph && lastEph->IOD() == newCorr->iod) {
241 newCorr->eph = lastEph;
242 }
243 else if (prevEph && prevEph->IOD() == newCorr->iod) {
244 newCorr->eph = prevEph;
245 }
246 else {
247 delete newCorr;
248 return;
249 }
250 }
251
252 // Process all older Epochs (if there are any)
253 // -------------------------------------------
254 const double waitTime = 5.0; // wait 5 sec
255 _processedBeforeTime = newCorr->tt - waitTime;
256
257 QList<cmbEpoch*> epochsToProcess;
258
259 QMapIterator<QString, cmbAC*> itAC(_ACs);
260 while (itAC.hasNext()) {
261 itAC.next();
262 cmbAC* AC = itAC.value();
263
264 QMutableListIterator<cmbEpoch*> itEpo(AC->epochs);
265 while (itEpo.hasNext()) {
266 cmbEpoch* epoch = itEpo.next();
267 if (epoch->time < _processedBeforeTime) {
268 epochsToProcess.append(epoch);
269 itEpo.remove();
270 }
271 }
272 }
273
274 if (epochsToProcess.size()) {
275 processEpochs(epochsToProcess);
276 }
277
278 // Check Modulo Time
279 // -----------------
280 const int moduloTime = 10;
281 if (int(newCorr->tt.gpssec()) % moduloTime != 0.0) {
282 delete newCorr;
283 return;
284 }
285
286 // Find/Create the instance of cmbEpoch class
287 // ------------------------------------------
288 cmbEpoch* newEpoch = 0;
289 QListIterator<cmbEpoch*> it(AC->epochs);
290 while (it.hasNext()) {
291 cmbEpoch* hlpEpoch = it.next();
292 if (hlpEpoch->time == newCorr->tt) {
293 newEpoch = hlpEpoch;
294 break;
295 }
296 }
297 if (newEpoch == 0) {
298 newEpoch = new cmbEpoch(AC->name);
299 newEpoch->time = newCorr->tt;
300 AC->epochs.append(newEpoch);
301 }
302
303 // Merge or add the correction
304 // ---------------------------
305 if (newEpoch->corr.find(newCorr->prn) != newEpoch->corr.end()) {
306 newEpoch->corr[newCorr->prn]->readLine(line); // merge (multiple messages)
307 }
308 else {
309 newEpoch->corr[newCorr->prn] = newCorr;
310 }
311}
312
313// Send results to caster
314////////////////////////////////////////////////////////////////////////////
315void bncComb::dumpResults(const bncTime& resTime,
316 const QMap<QString, t_corr*>& resCorr) {
317
318 _caster->open();
319
320 unsigned year, month, day;
321 resTime.civil_date (year, month, day);
322 double GPSweeks = resTime.gpssec();
323
324 struct ClockOrbit co;
325 memset(&co, 0, sizeof(co));
326 co.GPSEpochTime = (int)GPSweeks;
327 co.GLONASSEpochTime = (int)fmod(GPSweeks, 86400.0)
328 + 3 * 3600 - gnumleap(year, month, day);
329 co.ClockDataSupplied = 1;
330 co.OrbitDataSupplied = 1;
331 co.SatRefDatum = DATUM_ITRF;
332
333 QMapIterator<QString, t_corr*> it(resCorr);
334 while (it.hasNext()) {
335 it.next();
336 t_corr* corr = it.value();
337
338 struct ClockOrbit::SatData* sd = 0;
339 if (corr->prn[0] == 'G') {
340 sd = co.Sat + co.NumberOfGPSSat;
341 ++co.NumberOfGPSSat;
342 }
343 else if (corr->prn[0] == 'R') {
344 sd = co.Sat + CLOCKORBIT_NUMGPS + co.NumberOfGLONASSSat;
345 ++co.NumberOfGLONASSSat;
346 }
347
348 if (sd != 0) {
349 sd->ID = corr->prn.mid(1).toInt();
350 sd->IOD = corr->iod;
351 sd->Clock.DeltaA0 = corr->dClk * t_CST::c;
352 sd->Orbit.DeltaRadial = corr->rao(1);
353 sd->Orbit.DeltaAlongTrack = corr->rao(2);
354 sd->Orbit.DeltaCrossTrack = corr->rao(3);
355 sd->Orbit.DotDeltaRadial = corr->dotRao(1);
356 sd->Orbit.DotDeltaAlongTrack = corr->dotRao(2);
357 sd->Orbit.DotDeltaCrossTrack = corr->dotRao(3);
358 }
359
360 // SP3 Output
361 // ----------
362 if (_sp3) {
363 ColumnVector xc(4);
364 ColumnVector vv(3);
365 corr->eph->position(corr->tt.gpsw(), corr->tt.gpssec(),
366 xc.data(), vv.data());
367
368 _sp3->write(resTime.gpsw(), resTime.gpssec(), corr->prn, xc, _append);
369 }
370
371 delete corr;
372 }
373
374 // Send Corrections to Caster
375 // --------------------------
376 if ( _caster->usedSocket() &&
377 (co.NumberOfGPSSat > 0 || co.NumberOfGLONASSSat > 0) ) {
378 char obuffer[CLOCKORBIT_BUFFERSIZE];
379 int len = MakeClockOrbit(&co, COTYPE_AUTO, 0, obuffer, sizeof(obuffer));
380 if (len > 0) {
381 _caster->write(obuffer, len);
382 }
383 }
384
385 // Optionall send new Corrections to PPP client and/or write into file
386 // -------------------------------------------------------------------
387 RTCM3coDecoder::reopen(_outNameSkl, _outName, _out);
388 bncApp* app = (bncApp*) qApp;
389 if (app->_bncPPPclient || _out) {
390 QStringList corrLines;
391 co.messageType = COTYPE_GPSCOMBINED;
392 QStringListIterator il(RTCM3coDecoder::corrsToASCIIlines(resTime.gpsw(),
393 resTime.gpssec(), co, 0));
394 while (il.hasNext()) {
395 QString line = il.next();
396 if (_out) {
397 *_out << line.toAscii().data() << endl;
398 _out->flush();
399 }
400 line += " " + _caster->mountpoint();
401 corrLines << line;
402 }
403
404 if (app->_bncPPPclient) {
405 app->_bncPPPclient->slotNewCorrections(corrLines);
406 }
407 }
408}
409
410// Change the correction so that it refers to last received ephemeris
411////////////////////////////////////////////////////////////////////////////
412void bncComb::switchToLastEph(const t_eph* lastEph, t_corr* corr) {
413
414 ColumnVector oldXC(4);
415 ColumnVector oldVV(3);
416 corr->eph->position(corr->tt.gpsw(), corr->tt.gpssec(),
417 oldXC.data(), oldVV.data());
418
419 ColumnVector newXC(4);
420 ColumnVector newVV(3);
421 lastEph->position(corr->tt.gpsw(), corr->tt.gpssec(),
422 newXC.data(), newVV.data());
423
424 ColumnVector dX = newXC.Rows(1,3) - oldXC.Rows(1,3);
425 ColumnVector dV = newVV - oldVV;
426 double dC = newXC(4) - oldXC(4);
427
428 ColumnVector dRAO(3);
429 XYZ_to_RSW(newXC.Rows(1,3), newVV, dX, dRAO);
430
431 ColumnVector dDotRAO(3);
432 XYZ_to_RSW(newXC.Rows(1,3), newVV, dV, dDotRAO);
433
434 QString msg = "switch " + corr->prn
435 + QString(" %1 -> %2 %3").arg(corr->iod,3)
436 .arg(lastEph->IOD(),3).arg(dC*t_CST::c, 8, 'f', 4);
437
438 emit newMessage(msg.toAscii(), false);
439
440 corr->iod = lastEph->IOD();
441 corr->eph = lastEph;
442 corr->rao += dRAO;
443 corr->dotRao += dDotRAO;
444 corr->dClk -= dC;
445}
446
447// Process Epochs
448////////////////////////////////////////////////////////////////////////////
449void bncComb::processEpochs(const QList<cmbEpoch*>& epochs) {
450
451 _log.clear();
452
453 QTextStream out(&_log, QIODevice::WriteOnly);
454
455 out << "Combination:" << endl
456 << "------------------------------" << endl;
457
458 // Predict Parameters Values, Add White Noise
459 // ------------------------------------------
460 for (int iPar = 1; iPar <= _params.size(); iPar++) {
461 cmbParam* pp = _params[iPar-1];
462 if (pp->sig_P != 0.0) {
463 pp->xx = 0.0;
464 for (int jj = 1; jj <= _params.size(); jj++) {
465 _QQ(iPar, jj) = 0.0;
466 }
467 _QQ(iPar,iPar) = pp->sig_P * pp->sig_P;
468 }
469 }
470
471 bncTime resTime = epochs.first()->time;
472 QMap<QString, t_corr*> resCorr;
473
474 int nPar = _params.size();
475 int nObs = 0;
476
477 ColumnVector x0(nPar);
478 for (int iPar = 1; iPar <= _params.size(); iPar++) {
479 cmbParam* pp = _params[iPar-1];
480 x0(iPar) = pp->xx;
481 }
482
483 // Count Observations
484 // ------------------
485 QListIterator<cmbEpoch*> itEpo(epochs);
486 while (itEpo.hasNext()) {
487 cmbEpoch* epo = itEpo.next();
488 QMutableMapIterator<QString, t_corr*> itCorr(epo->corr);
489 while (itCorr.hasNext()) {
490 itCorr.next();
491 t_corr* corr = itCorr.value();
492
493 // Switch to last ephemeris
494 // ------------------------
495 t_eph* lastEph = _eph[corr->prn]->last;
496 if (lastEph == corr->eph) {
497 ++nObs;
498 }
499 else {
500 if (corr->eph == _eph[corr->prn]->prev) {
501 switchToLastEph(lastEph, corr);
502 ++nObs;
503 }
504 else {
505 itCorr.remove();
506 }
507 }
508 }
509 }
510
511 if (nObs > 0) {
512 Matrix AA(nObs, nPar);
513 ColumnVector ll(nObs);
514 DiagonalMatrix PP(nObs); PP = 1.0;
515
516 int iObs = 0;
517 QListIterator<cmbEpoch*> itEpo(epochs);
518 while (itEpo.hasNext()) {
519 cmbEpoch* epo = itEpo.next();
520 QMapIterator<QString, t_corr*> itCorr(epo->corr);
521
522 while (itCorr.hasNext()) {
523 itCorr.next();
524 ++iObs;
525 t_corr* corr = itCorr.value();
526
527 if (epo->acName == _masterAC) {
528 resCorr[corr->prn] = new t_corr(*corr);
529 }
530
531 for (int iPar = 1; iPar <= _params.size(); iPar++) {
532 cmbParam* pp = _params[iPar-1];
533 AA(iObs, iPar) = pp->partial(epo->acName, corr);
534 }
535
536 ll(iObs) = corr->dClk * t_CST::c - DotProduct(AA.Row(iObs), x0);
537 }
538
539 delete epo;
540 }
541
542 ColumnVector dx;
543 bncModel::kalman(AA, ll, PP, _QQ, dx);
544 ColumnVector vv = ll - AA * dx;
545
546 for (int iPar = 1; iPar <= _params.size(); iPar++) {
547 cmbParam* pp = _params[iPar-1];
548 pp->xx += dx(iPar);
549 if (pp->type == cmbParam::clk) {
550 if (resCorr.find(pp->prn) != resCorr.end()) {
551 resCorr[pp->prn]->dClk = pp->xx / t_CST::c;
552 }
553 }
554 out << currentDateAndTimeGPS().toString("yy-MM-dd hh:mm:ss ").toAscii().data();
555 out.setRealNumberNotation(QTextStream::FixedNotation);
556 out.setFieldWidth(8);
557 out.setRealNumberPrecision(4);
558 out << pp->toString() << " "
559 << pp->xx << " +- " << sqrt(_QQ(pp->index,pp->index)) << endl;
560 }
561 }
562
563 printResults(out, resTime, resCorr);
564 dumpResults(resTime, resCorr);
565
566 emit newMessage(_log, false);
567}
568
569// Print results to caster
570////////////////////////////////////////////////////////////////////////////
571void bncComb::printResults(QTextStream& out, const bncTime& resTime,
572 const QMap<QString, t_corr*>& resCorr) {
573
574 QMapIterator<QString, t_corr*> it(resCorr);
575 while (it.hasNext()) {
576 it.next();
577 t_corr* corr = it.value();
578 const t_eph* eph = corr->eph;
579 if (eph) {
580 double xx, yy, zz, cc;
581 eph->position(resTime.gpsw(), resTime.gpssec(), xx, yy, zz, cc);
582
583 out << currentDateAndTimeGPS().toString("yy-MM-dd hh:mm:ss ").toAscii().data();
584 out.setFieldWidth(3);
585 out << "Full Clock " << corr->prn << " " << corr->iod << " ";
586 out.setFieldWidth(14);
587 out << (cc + corr->dClk) * t_CST::c << endl;
588 }
589 else {
590 out << "bncComb::printResuls bug" << endl;
591 }
592 }
593}
Note: See TracBrowser for help on using the repository browser.