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

Last change on this file since 3046 was 3046, checked in by mervart, 13 years ago
File size: 15.9 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("sp3Path").toString().isEmpty() ) {
177 _sp3 = 0;
178 }
179 else {
180 QString prep = "BNS";
181 QString ext = ".sp3";
182 QString path = settings.value("sp3Path").toString();
183 QString intr = settings.value("sp3Intr").toString();
184 int sampl = settings.value("sp3Sampl").toInt();
185 _sp3 = new bnsSP3(prep, ext, path, intr, sampl);
186 }
187}
188
189// Destructor
190////////////////////////////////////////////////////////////////////////////
191bncComb::~bncComb() {
192 QMapIterator<QString, cmbAC*> it(_ACs);
193 while (it.hasNext()) {
194 it.next();
195 delete it.value();
196 }
197 delete _caster;
198 delete _out;
199 delete _sp3;
200}
201
202// Read and store one correction line
203////////////////////////////////////////////////////////////////////////////
204void bncComb::processCorrLine(const QString& staID, const QString& line) {
205 QMutexLocker locker(&_mutex);
206
207 // Find the relevant instance of cmbAC class
208 // -----------------------------------------
209 if (_ACs.find(staID) == _ACs.end()) {
210 return;
211 }
212 cmbAC* AC = _ACs[staID];
213
214 // Read the Correction
215 // -------------------
216 t_corr* newCorr = new t_corr();
217 if (!newCorr->readLine(line) == success) {
218 delete newCorr;
219 return;
220 }
221
222 // Reject delayed corrections
223 // --------------------------
224 if (_processedBeforeTime.valid() && newCorr->tt < _processedBeforeTime) {
225 delete newCorr;
226 return;
227 }
228
229 // Check the Ephemeris
230 //--------------------
231 if (_eph.find(newCorr->prn) == _eph.end()) {
232 delete newCorr;
233 return;
234 }
235 else {
236 t_eph* lastEph = _eph[newCorr->prn]->last;
237 t_eph* prevEph = _eph[newCorr->prn]->prev;
238 if (lastEph && lastEph->IOD() == newCorr->iod) {
239 newCorr->eph = lastEph;
240 }
241 else if (prevEph && prevEph->IOD() == newCorr->iod) {
242 newCorr->eph = prevEph;
243 }
244 else {
245 delete newCorr;
246 return;
247 }
248 }
249
250 // Process all older Epochs (if there are any)
251 // -------------------------------------------
252 const double waitTime = 5.0; // wait 5 sec
253 _processedBeforeTime = newCorr->tt - waitTime;
254
255 QList<cmbEpoch*> epochsToProcess;
256
257 QMapIterator<QString, cmbAC*> itAC(_ACs);
258 while (itAC.hasNext()) {
259 itAC.next();
260 cmbAC* AC = itAC.value();
261
262 QMutableListIterator<cmbEpoch*> itEpo(AC->epochs);
263 while (itEpo.hasNext()) {
264 cmbEpoch* epoch = itEpo.next();
265 if (epoch->time < _processedBeforeTime) {
266 epochsToProcess.append(epoch);
267 itEpo.remove();
268 }
269 }
270 }
271
272 if (epochsToProcess.size()) {
273 processEpochs(epochsToProcess);
274 }
275
276 // Check Modulo Time
277 // -----------------
278 const int moduloTime = 10;
279 if (int(newCorr->tt.gpssec()) % moduloTime != 0.0) {
280 delete newCorr;
281 return;
282 }
283
284 // Find/Create the instance of cmbEpoch class
285 // ------------------------------------------
286 cmbEpoch* newEpoch = 0;
287 QListIterator<cmbEpoch*> it(AC->epochs);
288 while (it.hasNext()) {
289 cmbEpoch* hlpEpoch = it.next();
290 if (hlpEpoch->time == newCorr->tt) {
291 newEpoch = hlpEpoch;
292 break;
293 }
294 }
295 if (newEpoch == 0) {
296 newEpoch = new cmbEpoch(AC->name);
297 newEpoch->time = newCorr->tt;
298 AC->epochs.append(newEpoch);
299 }
300
301 // Merge or add the correction
302 // ---------------------------
303 if (newEpoch->corr.find(newCorr->prn) != newEpoch->corr.end()) {
304 newEpoch->corr[newCorr->prn]->readLine(line); // merge (multiple messages)
305 }
306 else {
307 newEpoch->corr[newCorr->prn] = newCorr;
308 }
309}
310
311// Send results to caster
312////////////////////////////////////////////////////////////////////////////
313void bncComb::dumpResults(const bncTime& resTime,
314 const QMap<QString, t_corr*>& resCorr) {
315
316 _caster->open();
317
318 unsigned year, month, day;
319 resTime.civil_date (year, month, day);
320 double GPSweeks = resTime.gpssec();
321
322 struct ClockOrbit co;
323 memset(&co, 0, sizeof(co));
324 co.GPSEpochTime = (int)GPSweeks;
325 co.GLONASSEpochTime = (int)fmod(GPSweeks, 86400.0)
326 + 3 * 3600 - gnumleap(year, month, day);
327 co.ClockDataSupplied = 1;
328 co.OrbitDataSupplied = 1;
329 co.SatRefDatum = DATUM_ITRF;
330
331 QMapIterator<QString, t_corr*> it(resCorr);
332 while (it.hasNext()) {
333 it.next();
334 t_corr* corr = it.value();
335
336 struct ClockOrbit::SatData* sd = 0;
337 if (corr->prn[0] == 'G') {
338 sd = co.Sat + co.NumberOfGPSSat;
339 ++co.NumberOfGPSSat;
340 }
341 else if (corr->prn[0] == 'R') {
342 sd = co.Sat + CLOCKORBIT_NUMGPS + co.NumberOfGLONASSSat;
343 ++co.NumberOfGLONASSSat;
344 }
345
346 if (sd != 0) {
347 sd->ID = corr->prn.mid(1).toInt();
348 sd->IOD = corr->iod;
349 sd->Clock.DeltaA0 = corr->dClk * t_CST::c;
350 sd->Orbit.DeltaRadial = corr->rao(1);
351 sd->Orbit.DeltaAlongTrack = corr->rao(2);
352 sd->Orbit.DeltaCrossTrack = corr->rao(3);
353 sd->Orbit.DotDeltaRadial = corr->dotRao(1);
354 sd->Orbit.DotDeltaAlongTrack = corr->dotRao(2);
355 sd->Orbit.DotDeltaCrossTrack = corr->dotRao(3);
356 }
357
358 delete corr;
359 }
360
361 // Send Corrections to Caster
362 // --------------------------
363 if ( _caster->usedSocket() &&
364 (co.NumberOfGPSSat > 0 || co.NumberOfGLONASSSat > 0) ) {
365 char obuffer[CLOCKORBIT_BUFFERSIZE];
366 int len = MakeClockOrbit(&co, COTYPE_AUTO, 0, obuffer, sizeof(obuffer));
367 if (len > 0) {
368 _caster->write(obuffer, len);
369 }
370 }
371
372 // Optionall send new Corrections to PPP client and/or write into file
373 // -------------------------------------------------------------------
374 RTCM3coDecoder::reopen(_outNameSkl, _outName, _out);
375 bncApp* app = (bncApp*) qApp;
376 if (app->_bncPPPclient || _out) {
377 QStringList corrLines;
378 co.messageType = COTYPE_GPSCOMBINED;
379 QStringListIterator il(RTCM3coDecoder::corrsToASCIIlines(resTime.gpsw(),
380 resTime.gpssec(), co, 0));
381 while (il.hasNext()) {
382 QString line = il.next();
383 if (_out) {
384 *_out << line.toAscii().data() << endl;
385 _out->flush();
386 }
387 line += " " + _caster->mountpoint();
388 corrLines << line;
389 }
390
391 if (app->_bncPPPclient) {
392 app->_bncPPPclient->slotNewCorrections(corrLines);
393 }
394 }
395
396 //if (_sp3) {
397 // _sp3->write(GPSweek, GPSweeks, prn, xx, _append);
398 //}
399}
400
401// Change the correction so that it refers to last received ephemeris
402////////////////////////////////////////////////////////////////////////////
403void bncComb::switchToLastEph(const t_eph* lastEph, t_corr* corr) {
404
405 ColumnVector oldXC(4);
406 ColumnVector oldVV(3);
407 corr->eph->position(corr->tt.gpsw(), corr->tt.gpssec(),
408 oldXC.data(), oldVV.data());
409
410 ColumnVector newXC(4);
411 ColumnVector newVV(3);
412 lastEph->position(corr->tt.gpsw(), corr->tt.gpssec(),
413 newXC.data(), newVV.data());
414
415 ColumnVector dX = newXC.Rows(1,3) - oldXC.Rows(1,3);
416 ColumnVector dV = newVV - oldVV;
417 double dC = newXC(4) - oldXC(4);
418
419 ColumnVector dRAO(3);
420 XYZ_to_RSW(newXC.Rows(1,3), newVV, dX, dRAO);
421
422 ColumnVector dDotRAO(3);
423 XYZ_to_RSW(newXC.Rows(1,3), newVV, dV, dDotRAO);
424
425 QString msg = "switch " + corr->prn
426 + QString(" %1 -> %2 %3").arg(corr->iod,3)
427 .arg(lastEph->IOD(),3).arg(dC*t_CST::c, 8, 'f', 4);
428
429 emit newMessage(msg.toAscii(), false);
430
431 corr->iod = lastEph->IOD();
432 corr->eph = lastEph;
433 corr->rao += dRAO;
434 corr->dotRao += dDotRAO;
435 corr->dClk -= dC;
436}
437
438// Process Epochs
439////////////////////////////////////////////////////////////////////////////
440void bncComb::processEpochs(const QList<cmbEpoch*>& epochs) {
441
442 _log.clear();
443
444 QTextStream out(&_log, QIODevice::WriteOnly);
445
446 out << "Combination:" << endl
447 << "------------------------------" << endl;
448
449 // Predict Parameters Values, Add White Noise
450 // ------------------------------------------
451 for (int iPar = 1; iPar <= _params.size(); iPar++) {
452 cmbParam* pp = _params[iPar-1];
453 if (pp->sig_P != 0.0) {
454 pp->xx = 0.0;
455 for (int jj = 1; jj <= _params.size(); jj++) {
456 _QQ(iPar, jj) = 0.0;
457 }
458 _QQ(iPar,iPar) = pp->sig_P * pp->sig_P;
459 }
460 }
461
462 bncTime resTime = epochs.first()->time;
463 QMap<QString, t_corr*> resCorr;
464
465 int nPar = _params.size();
466 int nObs = 0;
467
468 ColumnVector x0(nPar);
469 for (int iPar = 1; iPar <= _params.size(); iPar++) {
470 cmbParam* pp = _params[iPar-1];
471 x0(iPar) = pp->xx;
472 }
473
474 // Count Observations
475 // ------------------
476 QListIterator<cmbEpoch*> itEpo(epochs);
477 while (itEpo.hasNext()) {
478 cmbEpoch* epo = itEpo.next();
479 QMutableMapIterator<QString, t_corr*> itCorr(epo->corr);
480 while (itCorr.hasNext()) {
481 itCorr.next();
482 t_corr* corr = itCorr.value();
483
484 // Switch to last ephemeris
485 // ------------------------
486 t_eph* lastEph = _eph[corr->prn]->last;
487 if (lastEph == corr->eph) {
488 ++nObs;
489 }
490 else {
491 if (corr->eph == _eph[corr->prn]->prev) {
492 switchToLastEph(lastEph, corr);
493 ++nObs;
494 }
495 else {
496 itCorr.remove();
497 }
498 }
499 }
500 }
501
502 if (nObs > 0) {
503 Matrix AA(nObs, nPar);
504 ColumnVector ll(nObs);
505 DiagonalMatrix PP(nObs); PP = 1.0;
506
507 int iObs = 0;
508 QListIterator<cmbEpoch*> itEpo(epochs);
509 while (itEpo.hasNext()) {
510 cmbEpoch* epo = itEpo.next();
511 QMapIterator<QString, t_corr*> itCorr(epo->corr);
512
513 while (itCorr.hasNext()) {
514 itCorr.next();
515 ++iObs;
516 t_corr* corr = itCorr.value();
517
518 if (epo->acName == _masterAC) {
519 resCorr[corr->prn] = new t_corr(*corr);
520 }
521
522 for (int iPar = 1; iPar <= _params.size(); iPar++) {
523 cmbParam* pp = _params[iPar-1];
524 AA(iObs, iPar) = pp->partial(epo->acName, corr);
525 }
526
527 ll(iObs) = corr->dClk * t_CST::c - DotProduct(AA.Row(iObs), x0);
528 }
529
530 delete epo;
531 }
532
533 ColumnVector dx;
534 bncModel::kalman(AA, ll, PP, _QQ, dx);
535 ColumnVector vv = ll - AA * dx;
536
537 for (int iPar = 1; iPar <= _params.size(); iPar++) {
538 cmbParam* pp = _params[iPar-1];
539 pp->xx += dx(iPar);
540 if (pp->type == cmbParam::clk) {
541 if (resCorr.find(pp->prn) != resCorr.end()) {
542 resCorr[pp->prn]->dClk = pp->xx / t_CST::c;
543 }
544 }
545 out << currentDateAndTimeGPS().toString("yy-MM-dd hh:mm:ss ").toAscii().data();
546 out.setRealNumberNotation(QTextStream::FixedNotation);
547 out.setFieldWidth(8);
548 out.setRealNumberPrecision(4);
549 out << pp->toString() << " "
550 << pp->xx << " +- " << sqrt(_QQ(pp->index,pp->index)) << endl;
551 }
552 }
553
554 printResults(out, resTime, resCorr);
555 dumpResults(resTime, resCorr);
556
557 emit newMessage(_log, false);
558}
559
560// Print results to caster
561////////////////////////////////////////////////////////////////////////////
562void bncComb::printResults(QTextStream& out, const bncTime& resTime,
563 const QMap<QString, t_corr*>& resCorr) {
564
565 QMapIterator<QString, t_corr*> it(resCorr);
566 while (it.hasNext()) {
567 it.next();
568 t_corr* corr = it.value();
569 const t_eph* eph = corr->eph;
570 if (eph) {
571 double xx, yy, zz, cc;
572 eph->position(resTime.gpsw(), resTime.gpssec(), xx, yy, zz, cc);
573
574 out << currentDateAndTimeGPS().toString("yy-MM-dd hh:mm:ss ").toAscii().data();
575 out.setFieldWidth(3);
576 out << "Full Clock " << corr->prn << " " << corr->iod << " ";
577 out.setFieldWidth(14);
578 out << (cc + corr->dClk) * t_CST::c << endl;
579 }
580 else {
581 out << "bncComb::printResuls bug" << endl;
582 }
583 }
584}
Note: See TracBrowser for help on using the repository browser.