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

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