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

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

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