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

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