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

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