source: ntrip/trunk/BNC/src/PPP/pppFilter.cpp@ 9421

Last change on this file since 9421 was 9421, checked in by stuerze, 3 years ago

update regarding PPP
  • Property svn:keywords set to Author Date Id Rev URL;svn:eol-style=native
  • Property svn:mime-type set to text/plain
File size: 28.6 KB
Line 
1/* -------------------------------------------------------------------------
2 * BKG NTRIP Client
3 * -------------------------------------------------------------------------
4 *
5 * Class: t_pppFilter
6 *
7 * Purpose: Filter Adjustment
8 *
9 * Author: L. Mervart
10 *
11 * Created: 29-Jul-2014
12 *
13 * Changes:
14 *
15 * -----------------------------------------------------------------------*/
16
17#include <iostream>
18#include <iomanip>
19#include <cmath>
20#include <newmat.h>
21#include <newmatio.h>
22#include <newmatap.h>
23
24#include "pppFilter.h"
25#include "bncutils.h"
26#include "pppParlist.h"
27#include "pppObsPool.h"
28#include "pppStation.h"
29
30using namespace BNC_PPP;
31using namespace std;
32
33// Constructor
34////////////////////////////////////////////////////////////////////////////
35t_pppFilter::t_pppFilter(t_pppObsPool* obsPool) {
36 _parlist = 0;
37 _numSat = 0;
38 _obsPool = obsPool;
39 _refPrn = t_prn();
40 _datumTrafo = new t_datumTrafo();
41}
42
43// Destructor
44////////////////////////////////////////////////////////////////////////////
45t_pppFilter::~t_pppFilter() {
46 delete _parlist;
47 delete _datumTrafo;
48}
49
50// Process Single Epoch
51////////////////////////////////////////////////////////////////////////////
52t_irc t_pppFilter::processEpoch() {
53 _numSat = 0;
54 const double maxSolGap = 60.0;
55
56 if (!_parlist) {
57 _parlist = new t_pppParlist();
58 }
59
60 // Vector of all Observations
61 // --------------------------
62 t_pppObsPool::t_epoch* epoch = _obsPool->lastEpoch();
63 if (!epoch) {
64 return failure;
65 }
66 vector<t_pppSatObs*>& allObs = epoch->obsVector();
67
68 // Time of the Epoch
69 // -----------------
70 _epoTime = epoch->epoTime();
71
72 if (!_firstEpoTime.valid() ||
73 !_lastEpoTimeOK.valid() ||
74 (maxSolGap > 0.0 && _epoTime - _lastEpoTimeOK > maxSolGap)) {
75 _firstEpoTime = _epoTime;
76 }
77
78 string epoTimeStr = string(_epoTime);
79
80 //--
81 // Set Parameters
82 // --------------
83 if (_parlist->set(_epoTime, allObs, _obsPool->getRefSatMap()) != success) {
84 return failure;
85 }
86 const vector<t_pppParam*>& params = _parlist->params();
87#ifdef BNC_DEBUG_PPP
88 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
89 LOG << params[iPar]->toString() << endl;
90 }
91#endif
92
93 // Status Vector, Variance-Covariance Matrix
94 // -----------------------------------------
95 ColumnVector xFltOld = _xFlt;
96 SymmetricMatrix QFltOld = _QFlt;
97
98 _QFlt.ReSize(_parlist->nPar()); _QFlt = 0.0;
99 _xFlt.ReSize(_parlist->nPar()); _xFlt = 0.0;
100 _x0.ReSize(_parlist->nPar()); _x0 = 0.0;
101
102 for (unsigned ii = 0; ii < params.size(); ii++) {
103 t_pppParam* par1 = params[ii];
104 if (QFltOld.size() == 0) {
105 par1->resetIndex();
106 }
107 _x0[ii] = par1->x0();
108 int iOld = par1->indexOld();
109 if (iOld < 0) {
110 _QFlt[ii][ii] = par1->sigma0() * par1->sigma0(); // new parameter
111 }
112 else {
113 _QFlt[ii][ii] = QFltOld[iOld][iOld] + par1->noise() * par1->noise();
114 _xFlt[ii] = xFltOld[iOld];
115 for (unsigned jj = 0; jj < ii; jj++) {
116 t_pppParam* par2 = params[jj];
117 int jOld = par2->indexOld();
118 if (jOld >= 0) {
119 _QFlt[ii][jj] = QFltOld(iOld+1,jOld+1);
120 }
121 }
122 }
123 }
124 predictCovCrdPart(QFltOld);
125
126 // Pre-Process Satellite Systems separately
127 // ----------------------------------------
128 bool preProcessing = false;
129 if (OPT->_obsModelType == OPT->DCMcodeBias ||
130 OPT->_obsModelType == OPT->DCMphaseBias) {
131 preProcessing = true;
132 unsigned usableSys = 0;
133 for (unsigned iSys = 0; iSys < OPT->systems().size(); iSys++) {
134 char sys = OPT->systems()[iSys];
135 _refPrn = (_obsPool->getRefSatMapElement(sys))->prn();
136 vector<t_pppSatObs*> obsVector;
137 for (unsigned jj = 0; jj < allObs.size(); jj++) {
138 if (allObs[jj]->prn().system() == sys) {
139 obsVector.push_back(allObs[jj]);
140 }
141 }
142 if (!obsVector.size()) {
143 continue;
144 }
145 else {
146 ++usableSys;
147 if (usableSys == 1) {
148 _datumTrafo->setFirstSystem(sys);
149 }
150 }
151 if (processSystem(OPT->LCs(sys), obsVector, _refPrn,
152 epoch->pseudoObsIono(), preProcessing) != success) {
153 _xFlt = xFltOld;
154 _QFlt = QFltOld;
155 return failure;
156 }
157 }
158 // refSat change required?
159 // -----------------------
160 if (_obsPool->refSatChangeRequired()) {
161 _xFlt = xFltOld;
162 _QFlt = QFltOld;
163 return success;
164 }
165 else if (!_obsPool->refSatChangeRequired()) {
166 initDatumTransformation(allObs, epoch->pseudoObsIono());
167 }
168 }
169
170 // Process Satellite Systems separately
171 // ------------------------------------
172 preProcessing = false;
173 unsigned usableSys = 0;
174 for (unsigned iSys = 0; iSys < OPT->systems().size(); iSys++) {
175 char sys = OPT->systems()[iSys];
176 if (OPT->_refSatRequired) {
177 _refPrn = (_obsPool->getRefSatMapElement(sys))->prn();
178 }
179 else {
180 _refPrn.set(sys, 0);
181 }
182 unsigned int num = 0;
183 vector<t_pppSatObs*> obsVector;
184 for (unsigned jj = 0; jj < allObs.size(); jj++) {
185 if (allObs[jj]->prn().system() == sys) {
186 obsVector.push_back(allObs[jj]);
187 ++num;
188 }
189 }
190 if (!num) {
191 continue;
192 }
193 else {
194 ++usableSys;
195 if (usableSys == 1 &&
196 OPT->_obsModelType == OPT->UncombPPP) {
197 _datumTrafo->setFirstSystem(sys);
198 }
199 }
200 LOG << epoTimeStr << " SATNUM " << sys << ' ' << right << setw(2) << num << endl;
201 if (processSystem(OPT->LCs(sys), obsVector, _refPrn,
202 epoch->pseudoObsIono(), preProcessing) != success) {
203 return failure;
204 }
205 }
206
207 // close epoch processing
208 // ----------------------
209 cmpDOP(allObs);
210 _parlist->printResult(_epoTime, _QFlt, _xFlt);
211 _lastEpoTimeOK = _epoTime; // remember time of last successful epoch processing
212 if (OPT->_refSatRequired) {
213 _obsPool->saveLastEpoRefSats();
214 _datumTrafo->setLastEpoParlist(_parlist);
215 }
216 return success;
217}
218
219// Process Selected LCs
220////////////////////////////////////////////////////////////////////////////
221t_irc t_pppFilter::processSystem(const vector<t_lc::type>& LCs,
222 const vector<t_pppSatObs*>& obsVector,
223 const t_prn& refPrn,
224 bool pseudoObsIonoAvailable,
225 bool preProcessing) {
226 LOG.setf(ios::fixed);
227 char sys = refPrn.system();
228
229 // Detect Cycle Slips
230 // ------------------
231 if (detectCycleSlips(LCs, obsVector, refPrn, preProcessing) != success) {
232 return failure;
233 }
234 if (preProcessing && _obsPool->refSatChangeRequired(sys)) {
235 return success;
236 }
237
238 ColumnVector xSav = _xFlt;
239 SymmetricMatrix QSav = _QFlt;
240 string epoTimeStr = string(_epoTime);
241 const vector<t_pppParam*>& params = _parlist->params();
242
243 unsigned usedLCs = LCs.size();
244 if (OPT->_pseudoObsIono && !pseudoObsIonoAvailable) {
245 usedLCs -= 1; // GIM not used
246 }
247 int hlpLCs = 0;
248 if (OPT->_pseudoObsTropo) {
249 hlpLCs = -1;
250 }
251 // max Obs
252 unsigned maxObs = obsVector.size() * (usedLCs + hlpLCs);
253 if (OPT->_pseudoObsTropo && _datumTrafo->firstSystem(sys)) {
254 maxObs += 1;
255 }
256 if (OPT->_pseudoObsIono && pseudoObsIonoAvailable) {
257 maxObs -= 1; // pseudo obs iono with respect to refSat
258 }
259
260 // Outlier Detection Loop
261 // ----------------------
262 for (unsigned iOutlier = 0; iOutlier < maxObs; iOutlier++) {
263
264 if (iOutlier > 0) {
265 _xFlt = xSav;
266 _QFlt = QSav;
267 }
268
269 // First-Design Matrix, Terms Observed-Computed, Weight Matrix
270 // -----------------------------------------------------------
271 Matrix AA(maxObs, _parlist->nPar());
272 ColumnVector ll(maxObs);
273 DiagonalMatrix PP(maxObs); PP = 0.0;
274
275 int iObs = -1;
276 vector<t_pppSatObs*> usedObs;
277 vector<t_lc::type> usedTypes;
278
279 // Real Observations
280 // =================
281 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
282 t_pppSatObs* obs = obsVector[ii];
283 if (iOutlier == 0 && !preProcessing) {
284 obs->resetOutlier();
285 }
286 if (!obs->outlier()) {
287 for (unsigned jj = 0; jj < usedLCs; jj++) {
288 const t_lc::type tLC = LCs[jj];
289 if (tLC == t_lc::GIM) {continue;}
290 if (tLC == t_lc::Tz0) {continue;}
291 ++iObs;
292 usedObs.push_back(obs);
293 usedTypes.push_back(tLC);
294 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
295 const t_pppParam* par = params[iPar];
296 AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
297 }
298 ll[iObs] = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA.Row(iObs+1));
299 PP[iObs] = 1.0 / (obs->sigma(tLC) * obs->sigma(tLC));
300 }
301 }
302 }
303
304 // pseudo Obs Tropo
305 // ================
306 if (OPT->_pseudoObsTropo && _datumTrafo->firstSystem(sys)) {
307 bool _pseudoObsTropoConsidered = false;
308 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
309 if (_pseudoObsTropoConsidered) {break;}
310 t_pppSatObs* obs = obsVector[ii];
311 for (unsigned jj = 0; jj < usedLCs; jj++) {
312 const t_lc::type tLC = LCs[jj];
313 if (tLC != t_lc::Tz0) {continue;}
314 ++iObs;
315 _pseudoObsTropoConsidered = true;
316 usedObs.push_back(obs);
317 usedTypes.push_back(tLC);
318 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
319 const t_pppParam* par = params[iPar];
320 AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
321 }
322 ll[iObs] = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA.Row(iObs+1));
323 PP[iObs] = 1.0 / (obs->sigma(tLC) * obs->sigma(tLC));
324 }
325 }
326 }
327
328 if ((!iOutlier) &&
329 (OPT->_obsModelType == OPT->DCMcodeBias ||
330 OPT->_obsModelType == OPT->DCMphaseBias) &&
331 (!preProcessing)) {
332 _datumTrafo->updateIndices(sys, iObs+1);
333 _datumTrafo->prepareAA(AA.SubMatrix(1, iObs+1 , 1, _parlist->nPar()), 1);
334 }
335
336 // Pseudo Obs Iono
337 // ================
338 if (OPT->_pseudoObsIono && pseudoObsIonoAvailable) {
339 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
340 t_pppSatObs* obs = obsVector[ii];
341 if (!obs->outlier()) {
342 for (unsigned jj = 0; jj < usedLCs; jj++) {
343 const t_lc::type tLC = LCs[jj];
344 if (tLC == t_lc::GIM && !obs->isReference()) {
345 ++iObs;
346 } else {continue;}
347 usedObs.push_back(obs);
348 usedTypes.push_back(tLC);
349 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
350 const t_pppParam* par = params[iPar];
351 AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
352 }
353 ll[iObs] = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA.Row(iObs+1));
354 PP[iObs] = 1.0 / (obs->sigma(tLC) * obs->sigma(tLC));
355 }
356 }
357 }
358 }
359
360 // Check number of observations, truncate matrices
361 // -----------------------------------------------
362 if (iObs == -1) {
363 return failure;
364 }
365 AA = AA.Rows(1, iObs+1);
366 ll = ll.Rows(1, iObs+1);
367 PP = PP.SymSubMatrix(1, iObs+1);
368
369 // Kalman update step
370 // ------------------
371 kalman(AA, ll, PP, _QFlt, _xFlt);
372
373 // Check Residuals
374 // ---------------
375 ColumnVector vv = AA * _xFlt - ll;
376 double maxOutlier = 0.0;
377 int maxOutlierIndex = -1;
378 t_lc::type maxOutlierLC = t_lc::dummy;
379 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
380 const t_lc::type tLC = usedTypes[ii];
381 double res = fabs(vv[ii]);
382 if (res > usedObs[ii]->maxRes(tLC)) {
383 if (res > fabs(maxOutlier)) {
384 maxOutlier = vv[ii];
385 maxOutlierIndex = ii;
386 maxOutlierLC = tLC;
387 }
388 }
389 }
390
391 // Mark outlier or break outlier detection loop
392 // --------------------------------------------
393 if (maxOutlierIndex > -1) {
394 t_pppSatObs* obs = usedObs[maxOutlierIndex];
395 t_pppParam* par = 0;
396#ifdef BNC_DEBUG_PPP
397 LOG << epoTimeStr << " Outlier ("
398 << ((preProcessing) ? "pre-processing) " : "fin-processing) ") << t_lc::toString(maxOutlierLC) << ' '
399 << obs->prn().toString() << ' '
400 << setw(8) << setprecision(4) << maxOutlier << endl;
401#endif
402 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
403 t_pppParam* hlp = params[iPar];
404 if (hlp->type() == t_pppParam::amb &&
405 hlp->prn() == obs->prn() &&
406 hlp->tLC() == usedTypes[maxOutlierIndex]) {
407 par = hlp;
408 }
409 }
410 if (preProcessing) {
411 // for refSats no ambiguity parameter exists
412 if ((obs->prn() == refPrn) &&
413 (t_lc::toString(maxOutlierLC) == "l1" ||
414 t_lc::toString(maxOutlierLC) == "l2") ) {
415 _obsPool->setRefSatChangeRequired(sys, true);
416 }
417 else {
418 if (par) {
419 par->setAmbResetCandidate();
420 obs->setOutlier();
421 }
422 else {
423 obs->setOutlier();
424 }
425 }
426 }
427 else {// fin-processing
428 if (par) {
429 if (par->ambResetCandidate()) {
430 resetAmb(par->prn(), obsVector, &QSav, &xSav);
431 }
432 else {
433 par->setAmbResetCandidate();
434 obs->setOutlier();
435 }
436 }
437 else {
438 obs->setOutlier();
439 }
440 }
441 }
442 // Print Residuals
443 // ---------------
444 else {
445 if (!preProcessing) {
446 for (unsigned jj = 0; jj < LCs.size(); jj++) {
447 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
448 const t_lc::type tLC = usedTypes[ii];
449 t_pppSatObs* obs = usedObs[ii];
450 if (tLC == LCs[jj]) {
451 obs->setRes(tLC, vv[ii]);
452 LOG << epoTimeStr << " RES "
453 << left << setw(3) << t_lc::toString(tLC) << right << ' ';
454 if (t_lc::toString(tLC) == "Tz0") {LOG << sys << " ";}
455 else {LOG << obs->prn().toString();}
456 LOG << setw(8) << setprecision(4) << vv[ii] << endl;
457 }
458 }
459 }
460 }
461 break;
462 }
463 }
464 return success;
465}
466
467// Cycle-Slip Detection
468////////////////////////////////////////////////////////////////////////////
469t_irc t_pppFilter::detectCycleSlips(const vector<t_lc::type>& LCs,
470 const vector<t_pppSatObs*>& obsVector,
471 const t_prn& refPrn,
472 bool preProcessing) {
473
474 const double SLIP = 20.0;
475 char sys = refPrn.system();
476 string epoTimeStr = string(_epoTime);
477 const vector<t_pppParam*>& params = _parlist->params();
478
479 for (unsigned ii = 0; ii < LCs.size(); ii++) {
480 const t_lc::type& tLC = LCs[ii];
481 if (t_lc::includesPhase(tLC)) {
482 for (unsigned iObs = 0; iObs < obsVector.size(); iObs++) {
483 const t_pppSatObs* obs = obsVector[iObs];
484
485 // Check set Slips and Jump Counters
486 // ---------------------------------
487 bool slip = false;
488
489 // in pre-processing only the reference satellite has to be checked
490 if (preProcessing && obs->prn() != refPrn) {
491 continue;
492 }
493
494 if (obs->slip()) {
495 LOG << epoTimeStr << "cycle slip set (obs) " << obs->prn().toString() << endl;
496 slip = true;
497 }
498
499 if (_slips[obs->prn()]._obsSlipCounter != -1 &&
500 _slips[obs->prn()]._obsSlipCounter != obs->slipCounter()) {
501 LOG << epoTimeStr << "cycle slip set (obsSlipCounter) " << obs->prn().toString() << endl;
502 slip = true;
503 }
504 if (!preProcessing) {
505 _slips[obs->prn()]._obsSlipCounter = obs->slipCounter();
506 }
507 if (_slips[obs->prn()]._biasJumpCounter != -1 &&
508 _slips[obs->prn()]._biasJumpCounter != obs->biasJumpCounter()) {
509 LOG << epoTimeStr << "cycle slip set (biasJumpCounter) " << obs->prn().toString() << endl;
510 slip = true;
511 }
512 if (!preProcessing) {
513 _slips[obs->prn()]._biasJumpCounter = obs->biasJumpCounter();
514 }
515 // Slip Set
516 // --------
517 if (slip) {
518 if (preProcessing) {
519 _obsPool->setRefSatChangeRequired(sys, true);
520 }
521 else {
522 resetAmb(obs->prn(), obsVector);
523 }
524 }
525 // Check Pre-Fit Residuals
526 // -----------------------
527 else {
528 if (refPrn != t_prn()) {return success;}
529 ColumnVector AA(params.size());
530 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
531 const t_pppParam* par = params[iPar];
532 AA[iPar] = par->partial(_epoTime, obs, tLC, refPrn);
533 }
534 double ll = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA);
535 double vv = DotProduct(AA, _xFlt) - ll;
536 if (fabs(vv) > SLIP) {
537 LOG << epoTimeStr << " cycle slip detected " << t_lc::toString(tLC) << ' '
538 << obs->prn().toString() << ' ' << setw(8) << setprecision(4) << vv << endl;
539 if (preProcessing) {
540 _obsPool->setRefSatChangeRequired(sys, true);
541 }
542 else {
543 resetAmb(obs->prn(), obsVector);
544 }
545 }
546 }
547 }
548 }
549 }
550 return success;
551}
552
553// Reset Ambiguity Parameter (cycle slip)
554////////////////////////////////////////////////////////////////////////////
555t_irc t_pppFilter::resetAmb(t_prn prn, const vector<t_pppSatObs*>& obsVector,
556 SymmetricMatrix* QSav, ColumnVector* xSav) {
557
558 t_irc irc = failure;
559 vector<t_pppParam*>& params = _parlist->params();
560 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
561 t_pppParam* par = params[iPar];
562 if (par->type() == t_pppParam::amb && par->prn() == prn) {
563 int ind = par->indexNew();
564 t_lc::type tLC = par->tLC();
565 LOG << string(_epoTime) << " RESET " << par->toString() << endl;
566 delete par; par = new t_pppParam(t_pppParam::amb, prn, tLC, &obsVector);
567 par->setIndex(ind);
568 params[iPar] = par;
569 for (unsigned ii = 1; ii <= params.size(); ii++) {
570 _QFlt(ii, ind+1) = 0.0;
571 if (QSav) {
572 (*QSav)(ii, ind+1) = 0.0;
573 }
574 }
575 _QFlt(ind+1,ind+1) = par->sigma0() * par->sigma0();
576 if (QSav) {
577 (*QSav)(ind+1,ind+1) = _QFlt(ind+1,ind+1);
578 }
579 _xFlt[ind] = 0.0;
580 if (xSav) {
581 (*xSav)[ind] = _xFlt[ind];
582 }
583 _x0[ind] = par->x0();
584 irc = success;
585 }
586 }
587
588 return irc;
589}
590
591// Add infinite noise to iono
592////////////////////////////////////////////////////////////////////////////
593t_irc t_pppFilter::addNoiseToIono(char sys) {
594
595 t_irc irc = failure;
596 vector<t_pppParam*>& params = _parlist->params();
597 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
598 t_pppParam* par = params[iPar];
599 if (par->type() == t_pppParam::ion && par->prn().system() == sys) {
600 int ind = par->indexNew();
601 LOG << string(_epoTime) << " ADD IONO_NOISE TO " << par->prn().toString() << endl;
602 par->setIndex(ind);
603 _QFlt(ind+1,ind+1) += par->sigma0() * par->sigma0();
604 irc = success;
605 }
606 }
607
608 return irc;
609}
610
611// Compute various DOP Values
612////////////////////////////////////////////////////////////////////////////
613void t_pppFilter::cmpDOP(const vector<t_pppSatObs*>& obsVector) {
614
615 _dop.reset();
616
617 try {
618 const unsigned numPar = 4;
619 Matrix AA(obsVector.size(), numPar);
620 _numSat = 0;
621 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
622 t_pppSatObs* obs = obsVector[ii];
623 char system = obs->prn().system();
624 t_prn refPrn = t_prn();
625 if (OPT->_refSatRequired) {
626 refPrn = _obsPool->getRefSatMapElement(system)->prn();
627 }
628 if (obs->isValid() && !obs->outlier()) {
629 ++_numSat;
630 for (unsigned iPar = 0; iPar < numPar; iPar++) {
631 const t_pppParam* par = _parlist->params()[iPar];
632 AA[_numSat-1][iPar] = par->partial(_epoTime, obs, t_lc::c1, refPrn);
633 }
634 }
635 }
636 if (_numSat < 4) {
637 return;
638 }
639 AA = AA.Rows(1, _numSat);
640 SymmetricMatrix NN; NN << AA.t() * AA;
641 SymmetricMatrix QQ = NN.i();
642
643 _dop.H = sqrt(QQ(1,1) + QQ(2,2));
644 _dop.V = sqrt(QQ(3,3));
645 _dop.P = sqrt(QQ(1,1) + QQ(2,2) + QQ(3,3));
646 _dop.T = sqrt(QQ(4,4));
647 _dop.G = sqrt(QQ(1,1) + QQ(2,2) + QQ(3,3) + QQ(4,4));
648 }
649 catch (...) {
650 }
651}
652
653// Compute various DOP Values
654////////////////////////////////////////////////////////////////////////////
655void t_pppFilter::predictCovCrdPart(const SymmetricMatrix& QFltOld) {
656
657 const vector<t_pppParam*>& params = _parlist->params();
658 if (params.size() < 3) {
659 return;
660 }
661
662 bool first = (params[0]->indexOld() < 0);
663
664 SymmetricMatrix Qneu(3); Qneu = 0.0;
665 for (unsigned ii = 0; ii < 3; ii++) {
666 const t_pppParam* par = params[ii];
667 if (first) {
668 Qneu[ii][ii] = par->sigma0() * par->sigma0();
669 }
670 else {
671 Qneu[ii][ii] = par->noise() * par->noise();
672 }
673 }
674
675 const t_pppStation* sta = PPP_CLIENT->staRover();
676 SymmetricMatrix Qxyz(3);
677 covariNEU_XYZ(Qneu, sta->ellApr().data(), Qxyz);
678
679 if (first) {
680 _QFlt.SymSubMatrix(1,3) = Qxyz;
681 }
682 else {
683 double dt = _epoTime - _firstEpoTime;
684 if (dt < OPT->_seedingTime) {
685 _QFlt.SymSubMatrix(1,3) = QFltOld.SymSubMatrix(1,3);
686 }
687 else {
688 _QFlt.SymSubMatrix(1,3) = QFltOld.SymSubMatrix(1,3) + Qxyz;
689 }
690 }
691}
692
693// Compute datum transformation
694////////////////////////////////////////////////////////////////////////////
695t_irc t_pppFilter::datumTransformation() {
696 // get last epoch
697 t_pppObsPool::t_epoch* epoch = _obsPool->lastEpoch();
698 if (!epoch) {
699 LOG << "!lastEpoch" << endl;
700 return failure;
701 }
702 else {
703 LOG.setf(ios::fixed);
704 LOG << string(epoch->epoTime()) << " DATUM TRANSFORMATION " << endl;
705 }
706
707 vector<t_pppSatObs*>& allObs = epoch->obsVector();
708
709 // reset old and set new refSats in last epoch (ambiguities/GIM)
710 // =============================================================
711 if (resetRefSatellitesLastEpoch(allObs) != true) {
712 LOG << "resetRefSatellitesLastEpoch = failure" << endl;
713 return failure;
714 }
715
716 if (OPT->_obsModelType == OPT->UncombPPP) {
717 return success;
718 }
719
720 // set AA2
721 // =======
722 t_pppParlist* parlist = _datumTrafo->lastEpoParlist();
723 if (parlist->set(epoch->epoTime(), allObs, _obsPool->getRefSatMap()) != success) {
724 return failure;
725 }
726 vector<t_pppParam*>& params = parlist->params();
727#ifdef BNC_DEBUG_PPP
728 LOG << " parameters of last epoch" << endl;
729 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
730 LOG << params[iPar]->toString() << "\t\t" << endl;
731 }
732#endif
733 unsigned nPar = parlist->nPar();
734 unsigned usableSys = 0;
735 for (unsigned iSys = 0; iSys < OPT->systems().size(); iSys++) {
736 char sys = OPT->systems()[iSys];
737 t_prn refPrn = (_obsPool->getRefSatMapElement(sys))->prn();
738 vector<t_pppSatObs*> obsVector;
739 for (unsigned jj = 0; jj < allObs.size(); jj++) {
740 if (allObs[jj]->prn().system() == sys) {
741 obsVector.push_back(allObs[jj]);
742 }
743 }
744 if (!obsVector.size()) {
745 continue;
746 }
747 else {
748 ++usableSys;
749 if (usableSys == 1) {
750 _datumTrafo->setFirstSystem(sys);
751 }
752 }
753 vector<t_lc::type> LCs = OPT->LCs(sys);
754 unsigned usedLCs = LCs.size();
755 if (OPT->_pseudoObsIono && !epoch->pseudoObsIono()) {
756 usedLCs -= 1; // GIM not used
757 }
758 int hlpLCs = 0;
759 if (OPT->_pseudoObsTropo) {
760 hlpLCs = -1;
761 }
762 // max Obs
763 unsigned maxObs = obsVector.size() * (usedLCs + hlpLCs);
764
765 if (OPT->_pseudoObsTropo && _datumTrafo->firstSystem(sys)) {
766 maxObs += 1;
767 }
768 if (OPT->_pseudoObsIono && epoch->pseudoObsIono()) {
769 maxObs -= 1; // pseudo obs iono with respect to refSat
770 }
771 Matrix AA(maxObs, nPar);
772
773 // Real Observations
774 // -----------------
775 int iObs = -1;
776 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
777 t_pppSatObs* obs = obsVector[ii];
778 for (unsigned jj = 0; jj < usedLCs; jj++) {
779 const t_lc::type tLC = LCs[jj];
780 if (tLC == t_lc::GIM) {continue;}
781 if (tLC == t_lc::Tz0) {continue;}
782 ++iObs;
783 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
784 const t_pppParam* par = params[iPar];
785 AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
786 }
787 }
788 }
789 // pseudo Obs Tropo
790 // ================
791 if (OPT->_pseudoObsTropo && _datumTrafo->firstSystem(sys)) {
792 bool pseudoObsTropoConsidered = false;
793 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
794 if (pseudoObsTropoConsidered) {break;}
795 t_pppSatObs* obs = obsVector[ii];
796 for (unsigned jj = 0; jj < usedLCs; jj++) {
797 const t_lc::type tLC = LCs[jj];
798 if (tLC != t_lc::Tz0) {continue;}
799 ++iObs;
800 pseudoObsTropoConsidered = true;
801 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
802 const t_pppParam* par = params[iPar];
803 AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
804 }
805 }
806 }
807 }
808 if (!iObs) {
809 continue;
810 }
811 _datumTrafo->updateIndices(sys, iObs+1);
812 _datumTrafo->prepareAA(AA.SubMatrix(1, iObs+1 , 1, nPar), 2);
813 }
814 _datumTrafo->updateNumObs();
815
816 // Datum Transformation
817 // ====================
818#ifdef BNC_DEBUG_PPP
819 //LOG << "AA1\n"; _datumTrafo->printMatrix(_datumTrafo->AA1(), _datumTrafo->numObs(), _datumTrafo->numPar());
820 //LOG << "AA2\n"; _datumTrafo->printMatrix(_datumTrafo->AA2(), _datumTrafo->numObs(), _datumTrafo->numPar());
821#endif
822 if(_datumTrafo->computeTrafoMatrix() != success) {
823 return failure;
824 }
825#ifdef BNC_DEBUG_PPP
826 //LOG << "D21" << endl; _datumTrafo->printMatrix(_datumTrafo->D21(), _datumTrafo->numObs(), _datumTrafo->numPar());
827#endif
828 ColumnVector xFltOld = _xFlt;
829 SymmetricMatrix QFltOld = _QFlt;
830
831 _QFlt << _datumTrafo->D21() * QFltOld * _datumTrafo->D21().t();
832 _xFlt = _datumTrafo->D21() * xFltOld;
833
834#ifdef BNC_DEBUG_PPP
835 LOG << "xFltOld:\n" << xFltOld << endl;
836 LOG << "xFlt :\n" << _xFlt << endl;
837#endif
838
839 // Reset Ambiguities after Datum Transformation
840 // ============================================
841 for (unsigned iSys = 0; iSys < OPT->systems().size(); iSys++) {
842 char sys = OPT->systems()[iSys];
843 t_prn refPrnOld = _obsPool->getRefSatMapElementLastEpoch(sys);
844 t_prn refPrnNew = (_obsPool->getRefSatMapElement(sys))->prn();
845 if (refPrnNew != refPrnOld) {
846 t_irc irc = resetAmb(_obsPool->getRefSatMapElementLastEpoch(sys), allObs);
847 if (OPT->_obsModelType == OPT->DCMcodeBias) {
848 if (irc == success) {
849 addNoiseToIono(sys);
850 }
851 }
852 }
853 }
854
855 // switch AA2 to AA1
856 // =================
857 _datumTrafo->switchAA();
858
859 // save parameter list
860 // ====================
861 _datumTrafo->setLastEpoParlist(_parlist);
862 return success;
863}
864
865// Init datum transformation
866////////////////////////////////////////////////////////////////////////////
867void t_pppFilter::initDatumTransformation(const std::vector<t_pppSatObs*>& allObs,
868 bool pseudoObsIono) {
869 unsigned trafoObs = 0;
870 for (unsigned iSys = 0; iSys < OPT->systems().size(); iSys++) {
871 char sys = OPT->systems()[iSys];
872 int satNum = 0;
873 for (unsigned jj = 0; jj < allObs.size(); jj++) {
874 if (allObs[jj]->prn().system() == sys) {
875 satNum++;
876 }
877 }
878 if (!satNum) {
879 continue;
880 }
881 // all LCs
882 unsigned realUsedLCs = OPT->LCs(sys).size();
883 // exclude pseudo obs GIM
884 if (OPT->_pseudoObsIono && !pseudoObsIono) {
885 realUsedLCs -= 1;
886 }
887 if (OPT->_pseudoObsTropo) {
888 realUsedLCs -= 1;
889 }
890 trafoObs += satNum * realUsedLCs;
891
892 if (OPT->_pseudoObsTropo && _datumTrafo->firstSystem(sys)) {
893 trafoObs += 1;
894 }
895 if (OPT->_pseudoObsIono && pseudoObsIono) {
896 trafoObs -= 1; // pseudo obs iono with respect to refSat
897 }
898 }
899 _datumTrafo->setNumObs(trafoObs);
900 _datumTrafo->setNumPar(_parlist->nPar());
901 _datumTrafo->initAA();
902}
903
904//
905//////////////////////////////////////////////////////////////////////////////
906bool t_pppFilter::resetRefSatellitesLastEpoch(std::vector<t_pppSatObs*>& obsVector) {
907
908 bool resetRefSat = false;
909 // reference satellite definition per system
910 for (unsigned iSys = 0; iSys < OPT->systems().size(); iSys++) {
911 char sys = OPT->systems()[iSys];
912 t_pppRefSat* refSat = _obsPool->getRefSatMapElement(sys);
913 t_prn newPrn = refSat->prn();
914 t_prn oldPrn = _obsPool->getRefSatMapElementLastEpoch(sys);
915#ifdef BNC_DEBUG_PPP
916 LOG << "oldPrn: " << oldPrn.toString() << " => newPrn: " << newPrn.toString() << endl;
917#endif
918 vector<t_pppSatObs*>::iterator it = obsVector.begin();
919 while (it != obsVector.end()) {
920 t_pppSatObs* satObs = *it;
921 if (satObs->prn() == newPrn) {
922 resetRefSat = true;
923 satObs->setAsReference();
924 }
925 else if (satObs->prn() == oldPrn) {
926 satObs->resetReference();
927 }
928 it++;
929 }
930 }
931 return resetRefSat;
932}
Note: See TracBrowser for help on using the repository browser.