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

Last change on this file since 8961 was 8961, checked in by stuerze, 4 years ago

PPP update: pseudo obs tropo added
  • Property svn:keywords set to Author Date Id Rev URL;svn:eol-style=native
  • Property svn:mime-type set to text/plain
File size: 19.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#include "pppClient.h"
30
31using namespace BNC_PPP;
32using namespace std;
33
34// Constructor
35////////////////////////////////////////////////////////////////////////////
36t_pppFilter::t_pppFilter(t_pppObsPool* obsPool) {
37 _parlist = 0;
38 _numSat = 0;
39 _obsPool = obsPool;
40 _refPrn = t_prn();
41 _datumTrafo = new t_datumTrafo();
42}
43
44// Destructor
45////////////////////////////////////////////////////////////////////////////
46t_pppFilter::~t_pppFilter() {
47 delete _parlist;
48 delete _datumTrafo;
49}
50
51// Process Single Epoch
52////////////////////////////////////////////////////////////////////////////
53t_irc t_pppFilter::processEpoch(int num) {
54 _numSat = 0;
55 _numEpoProcessing = num;
56 const double maxSolGap = 60.0;
57
58 if (!_parlist) {
59 _parlist = new t_pppParlist();
60 }
61
62 // Vector of all Observations
63 // --------------------------
64 t_pppObsPool::t_epoch* epoch = _obsPool->lastEpoch();
65 if (!epoch) {
66 return failure;
67 }
68 vector<t_pppSatObs*>& allObs = epoch->obsVector();
69
70 // Time of the Epoch
71 // -----------------
72 _epoTime = epoch->epoTime();
73
74 if (!_firstEpoTime.valid() ||
75 !_lastEpoTimeOK.valid() ||
76 (maxSolGap > 0.0 && _epoTime - _lastEpoTimeOK > maxSolGap)) {
77 _firstEpoTime = _epoTime;
78 }
79
80 string epoTimeStr = string(_epoTime);
81
82 //--
83 // Set Parameters
84 // --------------
85 _parlist->set(_epoTime, allObs, _obsPool->getRefSatMap());
86 const vector<t_pppParam*>& params = _parlist->params();
87
88 // Status Vector, Variance-Covariance Matrix
89 // -----------------------------------------
90 ColumnVector xFltOld = _xFlt;
91 SymmetricMatrix QFltOld = _QFlt;
92
93 _QFlt.ReSize(_parlist->nPar()); _QFlt = 0.0;
94 _xFlt.ReSize(_parlist->nPar()); _xFlt = 0.0;
95 _x0.ReSize(_parlist->nPar()); _x0 = 0.0;
96 for (unsigned ii = 0; ii < params.size(); ii++) {
97 const t_pppParam* par1 = params[ii];
98 _x0[ii] = par1->x0();
99 int iOld = par1->indexOld();
100 if (iOld < 0) {
101 _QFlt[ii][ii] = par1->sigma0() * par1->sigma0(); // new parameter
102 }
103 else {
104 _QFlt[ii][ii] = QFltOld[iOld][iOld] + par1->noise() * par1->noise();
105 _xFlt[ii] = xFltOld[iOld];
106 for (unsigned jj = 0; jj < ii; jj++) {
107 const t_pppParam* par2 = params[jj];
108 int jOld = par2->indexOld();
109 if (jOld >= 0) {
110 _QFlt[ii][jj] = QFltOld(iOld+1,jOld+1);
111 }
112 }
113 }
114 }
115 predictCovCrdPart(QFltOld);
116
117 // Init Datum Trafo
118 // ----------------------------------------
119 if ((OPT->_obsModelType == OPT->DCMcodeBias ||
120 OPT->_obsModelType == OPT->DCMphaseBias) && (_numEpoProcessing == 1)) {
121 _numAllUsedLCs = 0;
122 for (unsigned iSys = 0; iSys < OPT->systems().size(); iSys++) {
123 char system = OPT->systems()[iSys];
124 _numAllUsedLCs += OPT->LCs(system).size();
125 if (OPT->_pseudoObsIono && epoch->pseudoObsIono() == false) {
126 _numAllUsedLCs -= 1; // GIM not used
127 }
128 }
129 int modify = 0;
130 if (OPT->_pseudoObsTropo) {
131 modify = -1;
132 }
133 // max Obs
134 int maxObs = allObs.size() * (_numAllUsedLCs + modify);
135 if (OPT->_pseudoObsIono && epoch->pseudoObsIono() == true) { // stecDiff w.r.t refSat
136 maxObs -= 1;
137 }
138 if (OPT->_pseudoObsTropo) {
139 maxObs += 1; // once per station
140 }
141 _datumTrafo->initAA(maxObs, _parlist->nPar());
142 }
143
144 // Pre-Process Satellite Systems separately
145 // ----------------------------------------
146 bool preProcessing = false;
147 if (OPT->_obsModelType == OPT->DCMcodeBias ||
148 OPT->_obsModelType == OPT->DCMphaseBias) {
149 preProcessing = true;
150 for (unsigned iSys = 0; iSys < OPT->systems().size(); iSys++) {
151 (iSys) ? _datumTrafo->setFirstSystem(false) : _datumTrafo->setFirstSystem(true);
152 char system = OPT->systems()[iSys];
153 if (OPT->_refSatRequired) {
154 _refPrn = (_obsPool->getRefSatMapElement(system))->prn();
155 if (_obsPool->hasHistoricalRefSat(_refPrn)) {
156 LOG << epoTimeStr << " Warning: prevent to process erroneous refSat again!";
157 return failure;
158 }
159 }
160 vector<t_pppSatObs*> obsVector;
161 for (unsigned jj = 0; jj < allObs.size(); jj++) {
162 if (allObs[jj]->prn().system() == system) {
163 obsVector.push_back(allObs[jj]);
164 }
165 }
166 if (processSystem(OPT->LCs(system), obsVector, _refPrn,
167 epoch->pseudoObsIono(), preProcessing) != success) {
168 return failure;
169 }
170 }
171 // refSat change required?
172 // -----------------------
173 if (_obsPool->refSatChangeRequired()) {
174 _xFlt = xFltOld;
175 _QFlt = QFltOld;
176 return success;
177 }
178 else if (!_obsPool->refSatChangeRequired() &&
179 _numEpoProcessing > 1) {
180 datumTransformation(xFltOld, QFltOld);
181 }
182 }
183
184 // Process Satellite Systems separately
185 // ------------------------------------
186 preProcessing = false;
187 for (unsigned iSys = 0; iSys < OPT->systems().size(); iSys++) {
188 char system = OPT->systems()[iSys];
189 if (OPT->_refSatRequired) {
190 _refPrn = (_obsPool->getRefSatMapElement(system))->prn();
191 }
192 unsigned int num = 0;
193 vector<t_pppSatObs*> obsVector;
194 for (unsigned jj = 0; jj < allObs.size(); jj++) {
195 if (allObs[jj]->prn().system() == system) {
196 obsVector.push_back(allObs[jj]);
197 num++;
198 }
199 }
200 LOG << epoTimeStr << " SATNUM " << system << ' ' << right << setw(2) << num << endl;
201 if (processSystem(OPT->LCs(system), obsVector, _refPrn,
202 epoch->pseudoObsIono(), preProcessing) != success) {
203 return failure;
204 }
205 }
206
207 // close epoch processing
208 // ----------------------
209 cmpDOP(allObs);
210 _parlist->printResult(_epoTime, _QFlt, _xFlt);
211 _lastEpoTimeOK = _epoTime; // remember time of last successful epoch processing
212
213 return success;
214}
215
216// Process Selected LCs
217////////////////////////////////////////////////////////////////////////////
218t_irc t_pppFilter::processSystem(const vector<t_lc::type>& LCs,
219 const vector<t_pppSatObs*>& obsVector,
220 const t_prn& refPrn,
221 bool pseudoObsIonoAvailable,
222 bool preProcessing) {
223 LOG.setf(ios::fixed);
224
225 // Detect Cycle Slips
226 // ------------------
227
228 if (detectCycleSlips(LCs, obsVector, refPrn, preProcessing) != success) {
229 return failure;
230 }
231
232 unsigned usedLCs = LCs.size();
233 if (OPT->_pseudoObsIono && !pseudoObsIonoAvailable) {
234 usedLCs -= 1; // GIM not used
235 }
236
237 ColumnVector xSav = _xFlt;
238 SymmetricMatrix QSav = _QFlt;
239 string epoTimeStr = string(_epoTime);
240 const vector<t_pppParam*>& params = _parlist->params();
241 int modify = 0;
242 if (OPT->_pseudoObsTropo) {
243 modify = -1;
244 }
245 // max Obs
246 unsigned maxObs = obsVector.size() * (usedLCs + modify);
247 if (OPT->_pseudoObsIono && pseudoObsIonoAvailable) { // stecDiff w.r.t refSat
248 maxObs -= 1;
249 }
250 if (OPT->_pseudoObsTropo) {
251 maxObs +=1; // tropo pseudo obs once per station
252 }
253
254 // Outlier Detection Loop
255 // ----------------------
256 for (unsigned iOutlier = 0; iOutlier < maxObs; iOutlier++) {
257
258 if (iOutlier > 0) {
259 _xFlt = xSav;
260 _QFlt = QSav;
261 }
262
263 // First-Design Matrix, Terms Observed-Computed, Weight Matrix
264 // -----------------------------------------------------------
265 Matrix AA(maxObs, _parlist->nPar());
266 ColumnVector ll(maxObs);
267 DiagonalMatrix PP(maxObs); PP = 0.0;
268
269 int iObs = -1;
270 vector<t_pppSatObs*> usedObs;
271 vector<t_lc::type> usedTypes;
272 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
273 t_pppSatObs* obs = obsVector[ii];
274 if (!obs->outlier()) {
275 for (unsigned jj = 0; jj < usedLCs; jj++) {
276 const t_lc::type tLC = LCs[jj];
277 if (tLC == t_lc::GIM && obs->isReference()) {continue;}
278 if (tLC == t_lc::Tz0 && ii+1 != obsVector.size()) {continue;}
279 ++iObs;
280 usedObs.push_back(obs);
281 usedTypes.push_back(tLC);
282 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
283 const t_pppParam* par = params[iPar];
284 AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
285 }
286 ll[iObs] = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA.Row(iObs+1));
287 PP[iObs] = 1.0 / (obs->sigma(tLC) * obs->sigma(tLC));
288 }
289 }
290 }
291
292 if ((preProcessing && ! iOutlier) &&
293 (OPT->_obsModelType == OPT->DCMcodeBias || OPT->_obsModelType == OPT->DCMphaseBias)) {
294 _datumTrafo->updateIndices(maxObs);
295 _datumTrafo->prepareAA(AA, _numEpoProcessing, _parlist->nPar());
296 if (_obsPool->refSatChangeRequired()) { // from detectCycleSlips()
297 return success;
298 }
299 }
300
301 // Check number of observations, truncate matrices
302 // -----------------------------------------------
303 if (iObs == -1) {
304 return failure;
305 }
306 AA = AA.Rows(1, iObs+1);
307 ll = ll.Rows(1, iObs+1);
308 PP = PP.SymSubMatrix(1, iObs+1);
309
310 // Kalman update step
311 // ------------------
312 kalman(AA, ll, PP, _QFlt, _xFlt);
313
314 // Check Residuals
315 // ---------------
316 ColumnVector vv = AA * _xFlt - ll;
317 double maxOutlier = 0.0;
318 int maxOutlierIndex = -1;
319 t_lc::type maxOutlierLC = t_lc::dummy;
320 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
321 const t_lc::type tLC = usedTypes[ii];
322 double res = fabs(vv[ii]);
323 if (res > usedObs[ii]->maxRes(tLC)) {
324 if (res > fabs(maxOutlier)) {
325 maxOutlier = vv[ii];
326 maxOutlierIndex = ii;
327 maxOutlierLC = tLC;
328 }
329 }
330 }
331
332 // Mark outlier or break outlier detection loop
333 // --------------------------------------------
334 if (maxOutlierIndex > -1) {
335 t_pppSatObs* obs = usedObs[maxOutlierIndex];
336 t_pppParam* par = 0;
337 if (!preProcessing) {
338 LOG << epoTimeStr << " Outlier " << t_lc::toString(maxOutlierLC) << ' '
339 << obs->prn().toString() << ' '
340 << setw(8) << setprecision(4) << maxOutlier << endl;
341 }
342 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
343 t_pppParam* hlp = params[iPar];
344 if (hlp->type() == t_pppParam::amb &&
345 hlp->prn() == obs->prn() &&
346 hlp->tLC() == usedTypes[maxOutlierIndex]) {
347 par = hlp;
348 }
349 }
350 if (preProcessing) {
351 if (par && obs->prn() == refPrn) {
352 _obsPool->setRefSatChangeRequired(true);
353 break;
354 }
355 }
356 else {
357 if (par) {
358 if (par->ambResetCandidate() || _obsPool->hasHistoricalRefSat(par->prn())) {
359 resetAmb(par->prn(), obsVector, &QSav, &xSav);
360 }
361 else {
362 par->setAmbResetCandidate();
363 obs->setOutlier();
364 }
365 }
366 else {
367 obs->setOutlier();
368 }
369 }
370 }
371 // Print Residuals
372 // ---------------
373 else {
374 if (!preProcessing) {
375 for (unsigned jj = 0; jj < LCs.size(); jj++) {
376 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
377 const t_lc::type tLC = usedTypes[ii];
378 t_pppSatObs* obs = usedObs[ii];
379 if (tLC == LCs[jj]) {
380 obs->setRes(tLC, vv[ii]);
381 LOG << epoTimeStr << " RES "
382 << left << setw(3) << t_lc::toString(tLC) << right << ' '
383 << obs->prn().toString().substr(0,3) << ' '
384 << setw(8) << setprecision(4) << vv[ii] << endl;
385 }
386 }
387 }
388 }
389 break;
390 }
391 }
392
393 return success;
394}
395
396// Cycle-Slip Detection
397////////////////////////////////////////////////////////////////////////////
398t_irc t_pppFilter::detectCycleSlips(const vector<t_lc::type>& LCs,
399 const vector<t_pppSatObs*>& obsVector,
400 const t_prn& refPrn,
401 bool preProcessing) {
402 if (_obsPool->hasHistoricalRefSat(refPrn)) {
403 return success;
404 }
405
406 double SLIP = 20.0; // slip threshold
407 string epoTimeStr = string(_epoTime);
408 const vector<t_pppParam*>& params = _parlist->params();
409
410 if (preProcessing) {
411 SLIP += 10.0;
412 }
413
414 for (unsigned ii = 0; ii < LCs.size(); ii++) {
415 const t_lc::type& tLC = LCs[ii];
416 if (t_lc::includesPhase(tLC)) {
417 for (unsigned iObs = 0; iObs < obsVector.size(); iObs++) {
418 const t_pppSatObs* obs = obsVector[iObs];
419
420 if (preProcessing && obs->prn() != refPrn) {continue;}
421
422 // Check set Slips and Jump Counters
423 // ---------------------------------
424 bool slip = false;
425 if (obs->slip()) {
426 LOG << epoTimeStr << "cycle slip set (obs) " << obs->prn().toString() << endl;
427 slip = true;
428 }
429
430 if (_slips[obs->prn()]._obsSlipCounter != -1 &&
431 _slips[obs->prn()]._obsSlipCounter > obs->slipCounter()) {
432 LOG << epoTimeStr << "cycle slip set (obsSlipCounter) " << obs->prn().toString() << endl;
433 slip = true;
434 }
435 if (!preProcessing) {
436 _slips[obs->prn()]._obsSlipCounter = obs->slipCounter();
437 }
438 if (_slips[obs->prn()]._biasJumpCounter != -1 &&
439 _slips[obs->prn()]._biasJumpCounter != obs->biasJumpCounter()) {
440 LOG << epoTimeStr << "cycle slip set (biasJumpCounter) " << obs->prn().toString() << endl;
441 slip = true;
442 }
443 if (!preProcessing) {
444 _slips[obs->prn()]._biasJumpCounter = obs->biasJumpCounter();
445 }
446 // Slip Set
447 // --------
448 if (slip) {
449 if (preProcessing) {
450 _obsPool->setRefSatChangeRequired(true);
451 }
452 else {
453 resetAmb(obs->prn(), obsVector);
454 }
455 }
456 // Check Pre-Fit Residuals
457 // -----------------------
458 else {
459 ColumnVector AA(params.size());
460 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
461 const t_pppParam* par = params[iPar];
462 AA[iPar] = par->partial(_epoTime, obs, tLC, refPrn);
463 }
464
465 double ll = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA);
466 double vv = DotProduct(AA, _xFlt) - ll;
467 if (fabs(vv) > SLIP) {
468 if (preProcessing) {
469 _obsPool->setRefSatChangeRequired(true);
470 }
471 else {
472 LOG << epoTimeStr << " cycle slip detected " << t_lc::toString(tLC) << ' '
473 << obs->prn().toString() << ' ' << setw(8) << setprecision(4) << vv << endl;
474 resetAmb(obs->prn(), obsVector);
475 }
476 }
477 }
478 }
479 }
480 }
481 return success;
482}
483
484// Reset Ambiguity Parameter (cycle slip)
485////////////////////////////////////////////////////////////////////////////
486t_irc t_pppFilter::resetAmb(t_prn prn, const vector<t_pppSatObs*>& obsVector,
487 SymmetricMatrix* QSav, ColumnVector* xSav) {
488
489 t_irc irc = failure;
490 vector<t_pppParam*>& params = _parlist->params();
491 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
492 t_pppParam* par = params[iPar];
493 if (par->type() == t_pppParam::amb && par->prn() == prn) {
494 int ind = par->indexNew();
495 t_lc::type tLC = par->tLC();
496 LOG << string(_epoTime) << " RESET " << par->toString() << endl;
497 delete par; par = new t_pppParam(t_pppParam::amb, prn, tLC, &obsVector);
498 par->setIndex(ind);
499 params[iPar] = par;
500 for (unsigned ii = 1; ii <= params.size(); ii++) {
501 _QFlt(ii, ind+1) = 0.0;
502 if (QSav) {
503 (*QSav)(ii, ind+1) = 0.0;
504 }
505 }
506 _QFlt(ind+1,ind+1) = par->sigma0() * par->sigma0();
507 if (QSav) {
508 (*QSav)(ind+1,ind+1) = _QFlt(ind+1,ind+1);
509 }
510 _xFlt[ind] = 0.0;
511 if (xSav) {
512 (*xSav)[ind] = _xFlt[ind];
513 }
514 _x0[ind] = par->x0();
515 irc = success;
516 }
517 }
518
519 return irc;
520}
521
522// Compute various DOP Values
523////////////////////////////////////////////////////////////////////////////
524void t_pppFilter::cmpDOP(const vector<t_pppSatObs*>& obsVector) {
525
526 _dop.reset();
527
528 try {
529 const unsigned numPar = 4;
530 Matrix AA(obsVector.size(), numPar);
531 _numSat = 0;
532 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
533 t_pppSatObs* obs = obsVector[ii];
534 char system = obs->prn().system();
535 t_prn refPrn = t_prn();
536 if (OPT->_refSatRequired) {
537 refPrn = _obsPool->getRefSatMapElement(system)->prn();
538 }
539 if (obs->isValid() && !obs->outlier()) {
540 ++_numSat;
541 for (unsigned iPar = 0; iPar < numPar; iPar++) {
542 const t_pppParam* par = _parlist->params()[iPar];
543 AA[_numSat-1][iPar] = par->partial(_epoTime, obs, t_lc::c1, refPrn);
544 }
545 }
546 }
547 if (_numSat < 4) {
548 return;
549 }
550 AA = AA.Rows(1, _numSat);
551 SymmetricMatrix NN; NN << AA.t() * AA;
552 SymmetricMatrix QQ = NN.i();
553
554 _dop.H = sqrt(QQ(1,1) + QQ(2,2));
555 _dop.V = sqrt(QQ(3,3));
556 _dop.P = sqrt(QQ(1,1) + QQ(2,2) + QQ(3,3));
557 _dop.T = sqrt(QQ(4,4));
558 _dop.G = sqrt(QQ(1,1) + QQ(2,2) + QQ(3,3) + QQ(4,4));
559 }
560 catch (...) {
561 }
562}
563
564// Compute various DOP Values
565////////////////////////////////////////////////////////////////////////////
566void t_pppFilter::predictCovCrdPart(const SymmetricMatrix& QFltOld) {
567
568 const vector<t_pppParam*>& params = _parlist->params();
569 if (params.size() < 3) {
570 return;
571 }
572
573 bool first = (params[0]->indexOld() < 0);
574
575 SymmetricMatrix Qneu(3); Qneu = 0.0;
576 for (unsigned ii = 0; ii < 3; ii++) {
577 const t_pppParam* par = params[ii];
578 if (first) {
579 Qneu[ii][ii] = par->sigma0() * par->sigma0();
580 }
581 else {
582 Qneu[ii][ii] = par->noise() * par->noise();
583 }
584 }
585
586 const t_pppStation* sta = PPP_CLIENT->staRover();
587 SymmetricMatrix Qxyz(3);
588 covariNEU_XYZ(Qneu, sta->ellApr().data(), Qxyz);
589
590 if (first) {
591 _QFlt.SymSubMatrix(1,3) = Qxyz;
592 }
593 else {
594 double dt = _epoTime - _firstEpoTime;
595 if (dt < OPT->_seedingTime) {
596 _QFlt.SymSubMatrix(1,3) = QFltOld.SymSubMatrix(1,3);
597 }
598 else {
599 _QFlt.SymSubMatrix(1,3) = QFltOld.SymSubMatrix(1,3) + Qxyz;
600 }
601 }
602}
603
604// Compute datum transformation
605////////////////////////////////////////////////////////////////////////////
606void t_pppFilter::datumTransformation(const ColumnVector& xFltOld, const SymmetricMatrix& QFltOld) {
607 Matrix D21 = _datumTrafo->computeTrafoMatrix();
608 _QFlt << D21 * QFltOld * D21.t();
609 _xFlt = D21 * xFltOld;
610}
611
612
613// Reset Ambiguity Parameter (cycle slip)
614////////////////////////////////////////////////////////////////////////////
615t_irc t_pppFilter::addInfiniteNoise(t_pppParam::e_type para) {
616 t_irc irc = failure;
617 vector<t_pppParam*>& params = _parlist->params();
618 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
619 t_pppParam* par = params[iPar];
620 if (par->type() == para) {
621 int ind = par->indexNew();
622 if (ind < 0) {
623 return irc;
624 }
625 _QFlt(ind+1,ind+1) += par->sigma0() * par->sigma0();
626 irc = success;
627 }
628 }
629 return irc;
630}
Note: See TracBrowser for help on using the repository browser.