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

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