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

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

minor 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: 17.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#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);
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 _QFlt.ReSize(_parlist->nPar()); _QFlt = 0.0;
93 _xFlt.ReSize(_parlist->nPar()); _xFlt = 0.0;
94 _x0.ReSize(_parlist->nPar()); _x0 = 0.0;
95 for (unsigned ii = 0; ii < params.size(); ii++) {
96 const t_pppParam* par1 = params[ii];
97 _x0[ii] = par1->x0();
98 int iOld = par1->indexOld();
99 if (iOld < 0) {
100 _QFlt[ii][ii] = par1->sigma0() * par1->sigma0(); // new parameter
101 }
102 else {
103 _QFlt[ii][ii] = QFltOld[iOld][iOld] + par1->noise() * par1->noise();
104 _xFlt[ii] = xFltOld[iOld];
105 for (unsigned jj = 0; jj < ii; jj++) {
106 const t_pppParam* par2 = params[jj];
107 int jOld = par2->indexOld();
108 if (jOld >= 0) {
109 _QFlt[ii][jj] = QFltOld(iOld+1,jOld+1);
110 }
111 }
112 }
113 }
114 predictCovCrdPart(QFltOld);
115
116
117 // Pre-Process Satellite Systems separately
118 // ----------------------------------------
119 bool preProcessing = false;
120 if (OPT->_obsModelType == OPT->DCMcodeBias ||
121 OPT->_obsModelType == OPT->DCMphaseBias) {
122 preProcessing = true;
123 _numAllUsedLCs = 0;
124 for (unsigned iSys = 0; iSys < OPT->systems().size(); iSys++) {
125 char system = OPT->systems()[iSys];
126 _numAllUsedLCs += OPT->LCs(system).size();
127 if (OPT->_pseudoObsIono && !epoch->pseudoObsIono()) {
128 _numAllUsedLCs -= 1; // GIM not used
129 }
130 if (OPT->_refSatRequired) {
131 _refPrn = (_obsPool->getRefSatMapElement(system))->prn();
132 }
133 vector<t_pppSatObs*> obsVector;
134 for (unsigned jj = 0; jj < allObs.size(); jj++) {
135 if (allObs[jj]->prn().system() == system) {
136 obsVector.push_back(allObs[jj]);
137 }
138 }
139 if (processSystem(OPT->LCs(system), obsVector, _refPrn,
140 epoch->pseudoObsIono(), preProcessing) != success) {
141 return failure;
142 }
143 }
144 }
145
146 if (_numEpoProcessing == 1) {
147 int maxObs = allObs.size() * _numAllUsedLCs;
148 _datumTrafo->initAA(maxObs, _parlist->nPar());
149 }
150
151 // Process Satellite Systems separately
152 // ------------------------------------
153 preProcessing = false;
154 for (unsigned iSys = 0; iSys < OPT->systems().size(); iSys++) {
155 char system = OPT->systems()[iSys];
156 (iSys) ? _datumTrafo->setFirstSystem(false) :
157 _datumTrafo->setFirstSystem(true);
158 if (OPT->_refSatRequired) {
159 _refPrn = (_obsPool->getRefSatMapElement(system))->prn();
160 }
161 unsigned int num = 0;
162 vector<t_pppSatObs*> obsVector;
163 for (unsigned jj = 0; jj < allObs.size(); jj++) {
164 if (allObs[jj]->prn().system() == system) {
165 obsVector.push_back(allObs[jj]);
166 num++;
167 }
168 }
169 LOG << epoTimeStr << " SATNUM " << system << ' ' << right << setw(2) << num << endl;
170 if (processSystem(OPT->LCs(system), obsVector, _refPrn,
171 epoch->pseudoObsIono(), preProcessing) != success) {
172 return failure;
173 }
174 }
175
176 // refSat change required?
177 // -----------------------
178 if (_obsPool->refSatChangeRequired()) {
179 _xFlt = xFltOld;
180 _QFlt = QFltOld;
181 }
182 // close epoch processing
183 // ----------------------
184 else {
185 cmpDOP(allObs);
186 _parlist->printResult(_epoTime, _QFlt, _xFlt);
187 _lastEpoTimeOK = _epoTime; // remember time of last successful epoch processing
188 }
189
190 return success;
191}
192
193// Process Selected LCs
194////////////////////////////////////////////////////////////////////////////
195t_irc t_pppFilter::processSystem(const vector<t_lc::type>& LCs,
196 const vector<t_pppSatObs*>& obsVector,
197 const t_prn& refPrn,
198 bool pseudoObsIonoAvailable,
199 bool preProcessing) {
200 qDebug() << "======t_pppFilter::processSystem=======";
201 LOG.setf(ios::fixed);
202
203 // Detect Cycle Slips
204 // ------------------
205 if (detectCycleSlips(LCs, obsVector, refPrn, preProcessing) != success) {
206 return failure;
207 }
208
209 unsigned usedLCs = LCs.size(); //qDebug() << "usedLCs: " << usedLCs;
210 if (OPT->_pseudoObsIono && !pseudoObsIonoAvailable) {
211 usedLCs -= 1; // GIM not used
212 }
213 ColumnVector xSav = _xFlt;
214 SymmetricMatrix QSav = _QFlt;
215 string epoTimeStr = string(_epoTime);
216 const vector<t_pppParam*>& params = _parlist->params();
217 unsigned maxObs = obsVector.size() * usedLCs;
218 //unsigned maxObs = 2 * usedLCs;
219
220 if (OPT->_pseudoObsIono && pseudoObsIonoAvailable) { // stecDiff w.r.t refSat
221 maxObs -= 1;
222 }
223 qDebug() << "par.size() : " << _parlist->nPar() << " LCs.size(): " << usedLCs;
224 qDebug() << "obsVector.size(): " << obsVector.size() << " maxObs : " << maxObs;
225
226 // Outlier Detection Loop
227 // ----------------------
228 for (unsigned iOutlier = 0; iOutlier < maxObs; iOutlier++) {
229 qDebug() << "iOutlier: " << 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 qDebug() << "A(" << maxObs << "," << _parlist->nPar() << ")";
239 Matrix AA(maxObs, _parlist->nPar());
240 ColumnVector ll(maxObs);
241 DiagonalMatrix PP(maxObs); PP = 0.0;
242
243 // TETSPLOT
244 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
245 const t_pppParam* par = params[iPar];
246 cout << " " << par->toString() <<endl;
247 }
248 cout << endl;
249 //END TETSPLOT
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]; qDebug() << "SATELLITE: " << obs->prn().toString().c_str() << "isRef: " << obs->isReference();
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);
266 cout << setw(10) << par->partial(_epoTime, obs, tLC);
267 }
268 cout << endl;
269 ll[iObs] = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA.Row(iObs+1));
270 PP[iObs] = 1.0 / (obs->sigma(tLC) * obs->sigma(tLC));
271 //qDebug() << "ll[iObs]: " << ll[iObs];
272 //qDebug() << "PP[iObs]: " << PP[iObs];
273 }
274 }
275 }
276
277 if ((!preProcessing) &&
278 (OPT->_obsModelType == OPT->DCMcodeBias ||
279 OPT->_obsModelType == OPT->DCMphaseBias)) {
280 _datumTrafo->updateIndices(maxObs);
281 _datumTrafo->prepareAA(AA, _numEpoProcessing, _parlist->nPar());
282 }
283
284 // Check number of observations, truncate matrices
285 // -----------------------------------------------
286 if (iObs == -1) {
287 return failure;
288 }
289 AA = AA.Rows(1, iObs+1);
290 ll = ll.Rows(1, iObs+1);
291 PP = PP.SymSubMatrix(1, iObs+1);
292
293 // Kalman update step
294 // ------------------
295 kalman(AA, ll, PP, _QFlt, _xFlt);
296
297 // Check Residuals
298 // ---------------
299 ColumnVector vv = AA * _xFlt - ll;
300 double maxOutlier = 0.0;
301 int maxOutlierIndex = -1;
302 t_lc::type maxOutlierLC = t_lc::dummy;
303 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
304 const t_lc::type tLC = usedTypes[ii];
305 double res = fabs(vv[ii]);
306 if (res > usedObs[ii]->maxRes(tLC)) {
307 if (res > fabs(maxOutlier)) {
308 maxOutlier = vv[ii];
309 maxOutlierIndex = ii;
310 maxOutlierLC = tLC;
311 }
312 }
313 }
314
315 // Mark outlier or break outlier detection loop
316 // --------------------------------------------
317 if (maxOutlierIndex > -1) {
318 t_pppSatObs* obs = usedObs[maxOutlierIndex];
319 t_pppParam* par = 0;
320 if (!preProcessing) {
321 LOG << epoTimeStr << " Outlier " << t_lc::toString(maxOutlierLC) << ' '
322 << obs->prn().toString() << ' '
323 << setw(8) << setprecision(4) << maxOutlier << endl;
324 }
325 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
326 t_pppParam* hlp = params[iPar];
327 if (hlp->type() == t_pppParam::amb &&
328 hlp->prn() == obs->prn() &&
329 hlp->tLC() == usedTypes[maxOutlierIndex]) {
330 par = hlp;
331 }
332 }
333 if (par && preProcessing) {
334 if (par->prn() == refPrn) {
335 _obsPool->setRefSatChangeRequired(true);
336 }
337 }
338 else if (par && !preProcessing) {
339 if (par->ambResetCandidate()) {
340 resetAmb(par->prn(), obsVector, &QSav, &xSav);
341 }
342 else {
343 par->setAmbResetCandidate();
344 obs->setOutlier();
345 }
346 }
347 else if (!par && !preProcessing){
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
384 const double SLIP = 20.0; // slip threshold
385 string epoTimeStr = string(_epoTime);
386 const vector<t_pppParam*>& params = _parlist->params();
387
388 for (unsigned ii = 0; ii < LCs.size(); ii++) {
389 const t_lc::type& tLC = LCs[ii];
390 if (t_lc::includesPhase(tLC)) {
391 for (unsigned iObs = 0; iObs < obsVector.size(); iObs++) {
392 const t_pppSatObs* obs = obsVector[iObs];
393
394 // Check set Slips and Jump Counters
395 // ---------------------------------
396 bool slip = false;
397 if (obs->slip()) {
398 LOG << epoTimeStr << "cycle slip set (obs) " << obs->prn().toString() << endl;
399 slip = true;
400 }
401
402 if (_slips[obs->prn()]._obsSlipCounter != -1 &&
403 _slips[obs->prn()]._obsSlipCounter > obs->slipCounter()) {
404 LOG << epoTimeStr << "cycle slip set (obsSlipCounter) " << obs->prn().toString() << endl;
405 slip = true;
406 }
407 _slips[obs->prn()]._obsSlipCounter = obs->slipCounter();
408
409 if (_slips[obs->prn()]._biasJumpCounter != -1 &&
410 _slips[obs->prn()]._biasJumpCounter != obs->biasJumpCounter()) {
411 LOG << epoTimeStr << "cycle slip set (biasJumpCounter) " << obs->prn().toString() << endl;
412 slip = true;
413 }
414
415 // Slip Set
416 // --------
417 if (slip) {
418 if (preProcessing) {
419 if (obs->prn() == refPrn) {
420 _obsPool->setRefSatChangeRequired(true);
421 }
422 }
423 else {
424 resetAmb(obs->prn(), obsVector);
425 }
426 }
427 // Check Pre-Fit Residuals
428 // -----------------------
429 else {
430 ColumnVector AA(params.size());
431 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
432 const t_pppParam* par = params[iPar];
433 AA[iPar] = par->partial(_epoTime, obs, tLC);
434 }
435 double ll = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA);
436 double vv = DotProduct(AA, _xFlt) - ll;
437 if (fabs(vv) > SLIP) {
438 if (preProcessing) {
439 if (obs->prn() == refPrn) {
440 _obsPool->setRefSatChangeRequired(true);
441 }
442 }
443 else {
444 LOG << epoTimeStr << " cycle slip detected " << t_lc::toString(tLC) << ' '
445 << obs->prn().toString() << ' ' << setw(8) << setprecision(4) << vv << endl;
446 resetAmb(obs->prn(), obsVector);
447 }
448 }
449 }
450 }
451 }
452 }
453
454 return success;
455}
456
457// Reset Ambiguity Parameter (cycle slip)
458////////////////////////////////////////////////////////////////////////////
459t_irc t_pppFilter::resetAmb(t_prn prn, const vector<t_pppSatObs*>& obsVector,
460 SymmetricMatrix* QSav, ColumnVector* xSav) {
461 t_irc irc = failure;
462 vector<t_pppParam*>& params = _parlist->params();
463 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
464 t_pppParam* par = params[iPar];
465 if (par->type() == t_pppParam::amb && par->prn() == prn) {
466 int ind = par->indexNew();
467 t_lc::type tLC = par->tLC();
468 LOG << string(_epoTime) << " RESET " << par->toString() << endl;
469 delete par; par = new t_pppParam(t_pppParam::amb, prn, tLC, &obsVector);
470 par->setIndex(ind);
471 params[iPar] = par;
472 for (unsigned ii = 1; ii <= params.size(); ii++) {
473 _QFlt(ii, ind+1) = 0.0;
474 if (QSav) {
475 (*QSav)(ii, ind+1) = 0.0;
476 }
477 }
478 _QFlt(ind+1,ind+1) = par->sigma0() * par->sigma0();
479 if (QSav) {
480 (*QSav)(ind+1,ind+1) = _QFlt(ind+1,ind+1);
481 }
482 _xFlt[ind] = 0.0;
483 if (xSav) {
484 (*xSav)[ind] = _xFlt[ind];
485 }
486 _x0[ind] = par->x0();
487 irc = success;
488 }
489 }
490
491 return irc;
492}
493
494// Compute various DOP Values
495////////////////////////////////////////////////////////////////////////////
496void t_pppFilter::cmpDOP(const vector<t_pppSatObs*>& obsVector) {
497
498 _dop.reset();
499
500 try {
501 const unsigned numPar = 4;
502 Matrix AA(obsVector.size(), numPar);
503 _numSat = 0;
504 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
505 t_pppSatObs* obs = obsVector[ii];
506 if (obs->isValid() && !obs->outlier()) {
507 ++_numSat;
508 for (unsigned iPar = 0; iPar < numPar; iPar++) {
509 const t_pppParam* par = _parlist->params()[iPar];
510 AA[_numSat-1][iPar] = par->partial(_epoTime, obs, t_lc::c1);
511 }
512 }
513 }
514 if (_numSat < 4) {
515 return;
516 }
517 AA = AA.Rows(1, _numSat);
518 SymmetricMatrix NN; NN << AA.t() * AA;
519 SymmetricMatrix QQ = NN.i();
520
521 _dop.H = sqrt(QQ(1,1) + QQ(2,2));
522 _dop.V = sqrt(QQ(3,3));
523 _dop.P = sqrt(QQ(1,1) + QQ(2,2) + QQ(3,3));
524 _dop.T = sqrt(QQ(4,4));
525 _dop.G = sqrt(QQ(1,1) + QQ(2,2) + QQ(3,3) + QQ(4,4));
526 }
527 catch (...) {
528 }
529}
530
531// Compute various DOP Values
532////////////////////////////////////////////////////////////////////////////
533void t_pppFilter::predictCovCrdPart(const SymmetricMatrix& QFltOld) {
534
535 const vector<t_pppParam*>& params = _parlist->params();
536 if (params.size() < 3) {
537 return;
538 }
539
540 bool first = (params[0]->indexOld() < 0);
541
542 SymmetricMatrix Qneu(3); Qneu = 0.0;
543 for (unsigned ii = 0; ii < 3; ii++) {
544 const t_pppParam* par = params[ii];
545 if (first) {
546 Qneu[ii][ii] = par->sigma0() * par->sigma0();
547 }
548 else {
549 Qneu[ii][ii] = par->noise() * par->noise();
550 }
551 }
552
553 const t_pppStation* sta = PPP_CLIENT->staRover();
554 SymmetricMatrix Qxyz(3);
555 covariNEU_XYZ(Qneu, sta->ellApr().data(), Qxyz);
556
557 if (first) {
558 _QFlt.SymSubMatrix(1,3) = Qxyz;
559 }
560 else {
561 double dt = _epoTime - _firstEpoTime;
562 if (dt < OPT->_seedingTime) {
563 _QFlt.SymSubMatrix(1,3) = QFltOld.SymSubMatrix(1,3);
564 }
565 else {
566 _QFlt.SymSubMatrix(1,3) = QFltOld.SymSubMatrix(1,3) + Qxyz;
567 }
568 }
569}
570
571// Compute datum transformation
572////////////////////////////////////////////////////////////////////////////
573void t_pppFilter::datumTransformation(void) {
574 _QFlt = _datumTrafo->varCov(Q());
575}
576
Note: See TracBrowser for help on using the repository browser.