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

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