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

Last change on this file since 10328 was 10328, checked in by stuerze, 2 months ago

hDOP and vDOP values are now computed from the variances of the NEU components of the receiver position estimates

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