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

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