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

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