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

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