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

Last change on this file since 9508 was 9508, checked in by stuerze, 3 years ago

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