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

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