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

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

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