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

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