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

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