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

Last change on this file since 8956 was 8956, checked in by stuerze, 2 years ago

update regarding PPP

  • Property svn:keywords set to Author Date Id Rev URL;svn:eol-style=native
  • Property svn:mime-type set to text/plain
File size: 18.8 KB
Line 
1/* -------------------------------------------------------------------------
2 * BKG NTRIP Client
3 * -------------------------------------------------------------------------
4 *
5 * Class: t_pppFilter
6 *
7 * Purpose: Filter Adjustment
8 *
9 * Author: L. Mervart
10 *
11 * Created: 29-Jul-2014
12 *
13 * Changes:
14 *
15 * -----------------------------------------------------------------------*/
16
17#include <iostream>
18#include <iomanip>
19#include <cmath>
20#include <newmat.h>
21#include <newmatio.h>
22#include <newmatap.h>
23
24#include "pppFilter.h"
25#include "bncutils.h"
26#include "pppParlist.h"
27#include "pppObsPool.h"
28#include "pppStation.h"
29#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) &&
121 (_numEpoProcessing == 1)) {
122 _numAllUsedLCs = 0;
123 for (unsigned iSys = 0; iSys < OPT->systems().size(); iSys++) {
124 char system = OPT->systems()[iSys];
125 _numAllUsedLCs += OPT->LCs(system).size();
126 if (OPT->_pseudoObsIono && !epoch->pseudoObsIono()) {
127 _numAllUsedLCs -= 1; // GIM not used
128 }
129 }
130 int maxObs = allObs.size() * _numAllUsedLCs;
131 _datumTrafo->initAA(maxObs, _parlist->nPar());
132 }
133
134 // Pre-Process Satellite Systems separately
135 // ----------------------------------------
136 bool preProcessing = false;
137 if (OPT->_obsModelType == OPT->DCMcodeBias ||
138 OPT->_obsModelType == OPT->DCMphaseBias) {
139 preProcessing = true;
140 for (unsigned iSys = 0; iSys < OPT->systems().size(); iSys++) {
141 (iSys) ? _datumTrafo->setFirstSystem(false) : _datumTrafo->setFirstSystem(true);
142 char system = OPT->systems()[iSys];
143 if (OPT->_refSatRequired) {
144 _refPrn = (_obsPool->getRefSatMapElement(system))->prn();
145 if (_obsPool->hasHistoricalRefSat(_refPrn)) {
146 return failure;
147 }
148 }
149 vector<t_pppSatObs*> obsVector;
150 for (unsigned jj = 0; jj < allObs.size(); jj++) {
151 if (allObs[jj]->prn().system() == system) {
152 obsVector.push_back(allObs[jj]);
153 }
154 }
155 if (processSystem(OPT->LCs(system), obsVector, _refPrn,
156 epoch->pseudoObsIono(), preProcessing) != success) {
157 return failure;
158 }
159 }
160 // refSat change required?
161 // -----------------------
162 if (_obsPool->refSatChangeRequired()) {
163 _xFlt = xFltOld;
164 _QFlt = QFltOld;
165 return success;
166 }
167 else if (!_obsPool->refSatChangeRequired() &&
168 _numEpoProcessing > 1) {
169 datumTransformation(xFltOld, QFltOld);
170 }
171 }
172
173 // Process Satellite Systems separately
174 // ------------------------------------
175 preProcessing = false;
176 for (unsigned iSys = 0; iSys < OPT->systems().size(); iSys++) {
177 char system = OPT->systems()[iSys];
178 if (OPT->_refSatRequired) {
179 _refPrn = (_obsPool->getRefSatMapElement(system))->prn();
180 }
181 unsigned int num = 0;
182 vector<t_pppSatObs*> obsVector;
183 for (unsigned jj = 0; jj < allObs.size(); jj++) {
184 if (allObs[jj]->prn().system() == system) {
185 obsVector.push_back(allObs[jj]);
186 num++;
187 }
188 }
189 LOG << epoTimeStr << " SATNUM " << system << ' ' << right << setw(2) << num << endl;
190 if (processSystem(OPT->LCs(system), obsVector, _refPrn,
191 epoch->pseudoObsIono(), preProcessing) != success) {
192 return failure;
193 }
194 }
195
196 // close epoch processing
197 // ----------------------
198 cmpDOP(allObs);
199 _parlist->printResult(_epoTime, _QFlt, _xFlt);
200 _lastEpoTimeOK = _epoTime; // remember time of last successful epoch processing
201
202 return success;
203}
204
205// Process Selected LCs
206////////////////////////////////////////////////////////////////////////////
207t_irc t_pppFilter::processSystem(const vector<t_lc::type>& LCs,
208 const vector<t_pppSatObs*>& obsVector,
209 const t_prn& refPrn,
210 bool pseudoObsIonoAvailable,
211 bool preProcessing) {
212 LOG.setf(ios::fixed);
213
214 // Detect Cycle Slips
215 // ------------------
216
217 if (detectCycleSlips(LCs, obsVector, refPrn, preProcessing) != success) {
218 return failure;
219 }
220
221 unsigned usedLCs = LCs.size(); //qDebug() << "usedLCs: " << usedLCs;
222 if (OPT->_pseudoObsIono && !pseudoObsIonoAvailable) {
223 usedLCs -= 1; // GIM not used
224 }
225 ColumnVector xSav = _xFlt;
226 SymmetricMatrix QSav = _QFlt;
227 string epoTimeStr = string(_epoTime);
228 const vector<t_pppParam*>& params = _parlist->params();
229 unsigned maxObs = obsVector.size() * usedLCs;
230 //unsigned maxObs = 2 * usedLCs;
231
232 if (OPT->_pseudoObsIono && pseudoObsIonoAvailable) { // stecDiff w.r.t refSat
233 maxObs -= 1;
234 }
235
236 // Outlier Detection Loop
237 // ----------------------
238 for (unsigned iOutlier = 0; iOutlier < maxObs; iOutlier++) {
239
240 if (iOutlier > 0) {
241 _xFlt = xSav;
242 _QFlt = QSav;
243 }
244
245 // First-Design Matrix, Terms Observed-Computed, Weight Matrix
246 // -----------------------------------------------------------
247 Matrix AA(maxObs, _parlist->nPar());
248 ColumnVector ll(maxObs);
249 DiagonalMatrix PP(maxObs); PP = 0.0;
250
251 int iObs = -1;
252 vector<t_pppSatObs*> usedObs;
253 vector<t_lc::type> usedTypes;
254 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
255 t_pppSatObs* obs = obsVector[ii];
256 if (!obs->outlier()) {
257 for (unsigned jj = 0; jj < usedLCs; jj++) {
258 const t_lc::type tLC = LCs[jj];
259 if (tLC == t_lc::GIM && obs->isReference()) {continue;}
260 ++iObs;
261 usedObs.push_back(obs);
262 usedTypes.push_back(tLC);
263 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
264 const t_pppParam* par = params[iPar];
265 AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
266 }
267 ll[iObs] = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA.Row(iObs+1));
268 PP[iObs] = 1.0 / (obs->sigma(tLC) * obs->sigma(tLC));
269 }
270 }
271 }
272
273 if ((preProcessing && ! iOutlier) &&
274 (OPT->_obsModelType == OPT->DCMcodeBias || OPT->_obsModelType == OPT->DCMphaseBias)) {
275 _datumTrafo->updateIndices(maxObs);
276 _datumTrafo->prepareAA(AA, _numEpoProcessing, _parlist->nPar());
277 if (_obsPool->refSatChangeRequired()) { // from detectCycleSlips()
278 return success;
279 }
280 }
281
282 // Check number of observations, truncate matrices
283 // -----------------------------------------------
284 if (iObs == -1) {
285 return failure;
286 }
287 AA = AA.Rows(1, iObs+1);
288 ll = ll.Rows(1, iObs+1);
289 PP = PP.SymSubMatrix(1, iObs+1);
290
291 // Kalman update step
292 // ------------------
293 kalman(AA, ll, PP, _QFlt, _xFlt);
294
295 // Check Residuals
296 // ---------------
297 ColumnVector vv = AA * _xFlt - ll;
298 double maxOutlier = 0.0;
299 int maxOutlierIndex = -1;
300 t_lc::type maxOutlierLC = t_lc::dummy;
301 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
302 const t_lc::type tLC = usedTypes[ii];
303 double res = fabs(vv[ii]);
304 if (res > usedObs[ii]->maxRes(tLC)) {
305 if (res > fabs(maxOutlier)) {
306 maxOutlier = vv[ii];
307 maxOutlierIndex = ii;
308 maxOutlierLC = tLC;
309 }
310 }
311 }
312
313 // Mark outlier or break outlier detection loop
314 // --------------------------------------------
315 if (maxOutlierIndex > -1) {
316 t_pppSatObs* obs = usedObs[maxOutlierIndex];
317 t_pppParam* par = 0;
318 if (!preProcessing) {
319 LOG << epoTimeStr << " Outlier " << t_lc::toString(maxOutlierLC) << ' '
320 << obs->prn().toString() << ' '
321 << setw(8) << setprecision(4) << maxOutlier << endl;
322 }
323 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
324 t_pppParam* hlp = params[iPar];
325 if (hlp->type() == t_pppParam::amb &&
326 hlp->prn() == obs->prn() &&
327 hlp->tLC() == usedTypes[maxOutlierIndex]) {
328 par = hlp;
329 }
330 }
331 if (preProcessing) {
332 if (par && obs->prn() == refPrn) {
333 _obsPool->setRefSatChangeRequired(true);
334 break;
335 }
336 }
337 else {
338 if (par) {
339 if (par->ambResetCandidate() || _obsPool->hasHistoricalRefSat(par->prn())) {
340 resetAmb(par->prn(), obsVector, &QSav, &xSav);
341 }
342 else {
343 par->setAmbResetCandidate();
344 obs->setOutlier();
345 }
346 }
347 else {
348 obs->setOutlier();
349 }
350 }
351 }
352 // Print Residuals
353 // ---------------
354 else {
355 if (!preProcessing) {
356 for (unsigned jj = 0; jj < LCs.size(); jj++) {
357 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
358 const t_lc::type tLC = usedTypes[ii];
359 t_pppSatObs* obs = usedObs[ii];
360 if (tLC == LCs[jj]) {
361 obs->setRes(tLC, vv[ii]);
362 LOG << epoTimeStr << " RES "
363 << left << setw(3) << t_lc::toString(tLC) << right << ' '
364 << obs->prn().toString().substr(0,3) << ' '
365 << setw(8) << setprecision(4) << vv[ii] << endl;
366 }
367 }
368 }
369 }
370 break;
371 }
372 }
373
374 return success;
375}
376
377// Cycle-Slip Detection
378////////////////////////////////////////////////////////////////////////////
379t_irc t_pppFilter::detectCycleSlips(const vector<t_lc::type>& LCs,
380 const vector<t_pppSatObs*>& obsVector,
381 const t_prn& refPrn,
382 bool preProcessing) {
383 if (_obsPool->hasHistoricalRefSat(refPrn)) {
384 return success;
385 }
386
387 double SLIP = 20.0; // slip threshold
388 string epoTimeStr = string(_epoTime);
389 const vector<t_pppParam*>& params = _parlist->params();
390
391 if (preProcessing) {
392 SLIP += 10.0;
393 }
394
395 for (unsigned ii = 0; ii < LCs.size(); ii++) {
396 const t_lc::type& tLC = LCs[ii];
397 if (t_lc::includesPhase(tLC)) {
398 for (unsigned iObs = 0; iObs < obsVector.size(); iObs++) {
399 const t_pppSatObs* obs = obsVector[iObs];
400
401 if (preProcessing && obs->prn() != refPrn) {continue;}
402
403 // Check set Slips and Jump Counters
404 // ---------------------------------
405 bool slip = false;
406 if (obs->slip()) {
407 LOG << epoTimeStr << "cycle slip set (obs) " << obs->prn().toString() << endl;
408 slip = true;
409 }
410
411 if (_slips[obs->prn()]._obsSlipCounter != -1 &&
412 _slips[obs->prn()]._obsSlipCounter > obs->slipCounter()) {
413 LOG << epoTimeStr << "cycle slip set (obsSlipCounter) " << obs->prn().toString() << endl;
414 slip = true;
415 }
416 if (!preProcessing) {
417 _slips[obs->prn()]._obsSlipCounter = obs->slipCounter();
418 }
419
420 if (_slips[obs->prn()]._biasJumpCounter != -1 &&
421 _slips[obs->prn()]._biasJumpCounter != obs->biasJumpCounter()) {
422 LOG << epoTimeStr << "cycle slip set (biasJumpCounter) " << obs->prn().toString() << endl;
423 slip = true;
424 }
425 if (!preProcessing) {
426 _slips[obs->prn()]._biasJumpCounter = obs->biasJumpCounter();
427 }
428 // Slip Set
429 // --------
430 if (slip) {
431 if (preProcessing) {
432 _obsPool->setRefSatChangeRequired(true);
433 }
434 else {
435 resetAmb(obs->prn(), obsVector);
436 }
437 }
438 // Check Pre-Fit Residuals
439 // -----------------------
440 else {
441 ColumnVector AA(params.size());
442 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
443 const t_pppParam* par = params[iPar];
444 AA[iPar] = par->partial(_epoTime, obs, tLC, refPrn);
445 }
446
447 double ll = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA);
448 double vv = DotProduct(AA, _xFlt) - ll;
449 if (fabs(vv) > SLIP) {
450 if (preProcessing) {
451 _obsPool->setRefSatChangeRequired(true);
452 }
453 else {
454 LOG << epoTimeStr << " cycle slip detected " << t_lc::toString(tLC) << ' '
455 << obs->prn().toString() << ' ' << setw(8) << setprecision(4) << vv << endl;
456 resetAmb(obs->prn(), obsVector);
457 }
458 }
459 }
460 }
461 }
462 }
463 return success;
464}
465
466// Reset Ambiguity Parameter (cycle slip)
467////////////////////////////////////////////////////////////////////////////
468t_irc t_pppFilter::resetAmb(t_prn prn, const vector<t_pppSatObs*>& obsVector,
469 SymmetricMatrix* QSav, ColumnVector* xSav) {
470
471 t_irc irc = failure;
472 vector<t_pppParam*>& params = _parlist->params();
473 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
474 t_pppParam* par = params[iPar];
475 if (par->type() == t_pppParam::amb && par->prn() == prn) {
476 int ind = par->indexNew();
477 t_lc::type tLC = par->tLC();
478 LOG << string(_epoTime) << " RESET " << par->toString() << endl;
479 delete par; par = new t_pppParam(t_pppParam::amb, prn, tLC, &obsVector);
480 par->setIndex(ind);
481 params[iPar] = par;
482 for (unsigned ii = 1; ii <= params.size(); ii++) {
483 _QFlt(ii, ind+1) = 0.0;
484 if (QSav) {
485 (*QSav)(ii, ind+1) = 0.0;
486 }
487 }
488 _QFlt(ind+1,ind+1) = par->sigma0() * par->sigma0();
489 if (QSav) {
490 (*QSav)(ind+1,ind+1) = _QFlt(ind+1,ind+1);
491 }
492 _xFlt[ind] = 0.0;
493 if (xSav) {
494 (*xSav)[ind] = _xFlt[ind];
495 }
496 _x0[ind] = par->x0();
497 irc = success;
498 }
499 }
500
501 return irc;
502}
503
504// Compute various DOP Values
505////////////////////////////////////////////////////////////////////////////
506void t_pppFilter::cmpDOP(const vector<t_pppSatObs*>& obsVector) {
507
508 _dop.reset();
509
510 try {
511 const unsigned numPar = 4;
512 Matrix AA(obsVector.size(), numPar);
513 _numSat = 0;
514 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
515 t_pppSatObs* obs = obsVector[ii];
516 char system = obs->prn().system();
517 t_prn refPrn = t_prn();
518 if (OPT->_refSatRequired) {
519 refPrn = _obsPool->getRefSatMapElement(system)->prn();
520 }
521 if (obs->isValid() && !obs->outlier()) {
522 ++_numSat;
523 for (unsigned iPar = 0; iPar < numPar; iPar++) {
524 const t_pppParam* par = _parlist->params()[iPar];
525 AA[_numSat-1][iPar] = par->partial(_epoTime, obs, t_lc::c1, refPrn);
526 }
527 }
528 }
529 if (_numSat < 4) {
530 return;
531 }
532 AA = AA.Rows(1, _numSat);
533 SymmetricMatrix NN; NN << AA.t() * AA;
534 SymmetricMatrix QQ = NN.i();
535
536 _dop.H = sqrt(QQ(1,1) + QQ(2,2));
537 _dop.V = sqrt(QQ(3,3));
538 _dop.P = sqrt(QQ(1,1) + QQ(2,2) + QQ(3,3));
539 _dop.T = sqrt(QQ(4,4));
540 _dop.G = sqrt(QQ(1,1) + QQ(2,2) + QQ(3,3) + QQ(4,4));
541 }
542 catch (...) {
543 }
544}
545
546// Compute various DOP Values
547////////////////////////////////////////////////////////////////////////////
548void t_pppFilter::predictCovCrdPart(const SymmetricMatrix& QFltOld) {
549
550 const vector<t_pppParam*>& params = _parlist->params();
551 if (params.size() < 3) {
552 return;
553 }
554
555 bool first = (params[0]->indexOld() < 0);
556
557 SymmetricMatrix Qneu(3); Qneu = 0.0;
558 for (unsigned ii = 0; ii < 3; ii++) {
559 const t_pppParam* par = params[ii];
560 if (first) {
561 Qneu[ii][ii] = par->sigma0() * par->sigma0();
562 }
563 else {
564 Qneu[ii][ii] = par->noise() * par->noise();
565 }
566 }
567
568 const t_pppStation* sta = PPP_CLIENT->staRover();
569 SymmetricMatrix Qxyz(3);
570 covariNEU_XYZ(Qneu, sta->ellApr().data(), Qxyz);
571
572 if (first) {
573 _QFlt.SymSubMatrix(1,3) = Qxyz;
574 }
575 else {
576 double dt = _epoTime - _firstEpoTime;
577 if (dt < OPT->_seedingTime) {
578 _QFlt.SymSubMatrix(1,3) = QFltOld.SymSubMatrix(1,3);
579 }
580 else {
581 _QFlt.SymSubMatrix(1,3) = QFltOld.SymSubMatrix(1,3) + Qxyz;
582 }
583 }
584}
585
586// Compute datum transformation
587////////////////////////////////////////////////////////////////////////////
588void t_pppFilter::datumTransformation(const ColumnVector& xFltOld, const SymmetricMatrix& QFltOld) {
589
590 Matrix D21 = _datumTrafo->computeTrafoMatrix();
591 _QFlt << D21 * QFltOld * D21.t();
592 _xFlt = D21 * xFltOld;
593}
594
595
596// Reset Ambiguity Parameter (cycle slip)
597////////////////////////////////////////////////////////////////////////////
598t_irc t_pppFilter::addInfiniteNoise(t_pppParam::e_type para) {
599 t_irc irc = failure;
600 vector<t_pppParam*>& params = _parlist->params();
601 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
602 t_pppParam* par = params[iPar];
603 if (par->type() == para) {
604 int ind = par->indexNew();
605 if (ind < 0) {
606 return irc;
607 }
608 _QFlt(ind+1,ind+1) += par->sigma0() * par->sigma0();
609 irc = success;
610 }
611 }
612 return irc;
613}
Note: See TracBrowser for help on using the repository browser.