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

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