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

Last change on this file since 9532 was 9532, checked in by stuerze, 2 years ago

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