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

Last change on this file since 10381 was 10381, checked in by stuerze, 4 months ago

minor changes
  • 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.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() {
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 QMap<char, int> &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 (auto it = usedSystems.begin(); it != usedSystems.end(); ++it) {
95 char sys = it.key();
96 unsigned 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 offGps = 0.0;
188 if (sys == 'G' && tLC != t_lc::MW) {
189 offGps = PPP_CLIENT->offGps();
190 }
191 double offGlo = 0.0;
192 if (sys == 'R' && tLC != t_lc::MW) {
193 offGlo = PPP_CLIENT->offGlo();
194 }
195 double offGal = 0.0;
196 if (sys == 'E' && tLC != t_lc::MW) {
197 offGal = PPP_CLIENT->offGal();
198 }
199 double offBds = 0.0;
200 if (sys == 'C' && tLC != t_lc::MW) {
201 offBds = PPP_CLIENT->offBds();
202 }
203 ll[iObs] = obs->obsValue(tLC) -offGps - offGlo - offGal - offBds - obs->cmpValue(tLC) - DotProduct(_x0, AA.Row(iObs + 1));
204 PP[iObs] = 1.0 / (obs->sigma(tLC) * obs->sigma(tLC));
205 }
206 }
207 }
208
209 // Check number of observations
210 // ----------------------------
211 if (!nSat) {
212 return failure;
213 }
214
215 // Pseudo Obs Iono
216 // ================
217 if (OPT->_pseudoObsIono && pseudoObsIonoAvailable) {
218 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
219 t_pppSatObs *obs = obsVector[ii];
220 char sys = obs->prn().system();
221 if (!obs->outlier()) {
222 for (unsigned jj = 0; jj < usedLCs; jj++) {
223 const t_lc::type tLC = LCs[jj];
224 if (tLC == t_lc::GIM) {
225 ++iObs;
226 } else {
227 continue;
228 }
229 usedObs.push_back(obs);
230 usedTypes.push_back(tLC);
231 for (unsigned iPar = 0; iPar < nPar; iPar++) {
232 const t_pppParam *par = params[iPar];
233 AA[iObs][iPar] = par->partial(_epoTime, obs, tLC);
234 }
235 double offGps = 0.0;
236 if (sys == 'G' && tLC != t_lc::MW) {
237 offGps = PPP_CLIENT->offGps();
238 }
239 double offGlo = 0.0;
240 if (sys == 'R' && tLC != t_lc::MW) {
241 offGlo = PPP_CLIENT->offGlo();
242 }
243 double offGal = 0.0;
244 if (sys == 'E' && tLC != t_lc::MW) {
245 offGal = PPP_CLIENT->offGal();
246 }
247 double offBds = 0.0;
248 if (sys == 'C' && tLC != t_lc::MW) {
249 offBds = PPP_CLIENT->offBds();
250 }
251 ll[iObs] = obs->obsValue(tLC) -offGps - offGlo - offGal - offBds - obs->cmpValue(tLC) - DotProduct(_x0, AA.Row(iObs + 1));
252 PP[iObs] = 1.0 / (obs->sigma(tLC) * obs->sigma(tLC));
253 }
254 }
255 }
256 }
257
258 // Truncate matrices
259 // -----------------
260 AA = AA.Rows(1, iObs + 1);
261 ll = ll.Rows(1, iObs + 1);
262 PP = PP.SymSubMatrix(1, iObs + 1);
263
264 // Kalman update step
265 // ------------------
266 kalman(AA, ll, PP, _QFlt, _xFlt);
267
268 // Check Residuals
269 // ---------------
270 ColumnVector vv = AA * _xFlt - ll;
271 double maxOutlier = 0.0;
272 int maxOutlierIndex = -1;
273 t_lc::type maxOutlierLC = t_lc::dummy;
274 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
275 const t_lc::type tLC = usedTypes[ii];
276 double res = fabs(vv[ii]);
277 if (res > usedObs[ii]->maxRes(tLC)) {
278 if (res > fabs(maxOutlier)) {
279 maxOutlier = vv[ii];
280 maxOutlierIndex = ii;
281 maxOutlierLC = tLC;
282 }
283 }
284 }
285
286 // Mark outlier or break outlier detection loop
287 // --------------------------------------------
288 if (maxOutlierIndex > -1) {
289 t_pppSatObs *obs = usedObs[maxOutlierIndex];
290 t_pppParam *par = 0;
291 LOG << epoTimeStr << " Outlier " << t_lc::toString(maxOutlierLC) << ' '
292 << obs->prn().toString() << ' ' << setw(8) << setprecision(4)
293 << maxOutlier << endl;
294 for (unsigned iPar = 0; iPar < nPar; iPar++) {
295 t_pppParam *hlp = params[iPar];
296 if (hlp->type() == t_pppParam::amb &&
297 hlp->prn() == obs->prn() &&
298 hlp->tLC() == usedTypes[maxOutlierIndex]) {
299 par = hlp;
300 }
301 }
302 if (par) {
303 resetAmb(obs->prn(), obsVector, maxOutlierLC, &QSav, &xSav);
304 }
305 else {
306 obs->setOutlier();
307 }
308 }
309 // Print Residuals
310 // ---------------
311 else {
312 for (unsigned jj = 0; jj < LCs.size(); jj++) {
313 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
314 const t_lc::type tLC = usedTypes[ii];
315 t_pppSatObs *obs = usedObs[ii];
316 if (tLC == LCs[jj]) {
317 obs->setRes(tLC, vv[ii]);
318 LOG << epoTimeStr << " RES " << left << setw(3)
319 << t_lc::toString(tLC) << right << ' '
320 << obs->prn().toString() << ' '
321 << setw(8) << setprecision(4) << vv[ii] << endl;
322 }
323 }
324 }
325 break;
326 }
327 }
328 return success;
329}
330
331// Cycle-Slip Detection
332////////////////////////////////////////////////////////////////////////////
333t_irc t_pppFilter::detectCycleSlips(const vector<t_lc::type> &LCs,
334 const vector<t_pppSatObs*> &obsVector) {
335
336 double SLIP = 20.0;
337 double fac = 1.0;
338 if (_lastEpoTimeOK.valid()) {
339 fac = _epoTime - _lastEpoTimeOK;
340 if (fac > 60.0 || fac < 1.0) {
341 fac = 1.0;
342 }
343 }
344 SLIP *= fac;
345 string epoTimeStr = string(_epoTime);
346 const vector<t_pppParam*> &params = _parlist->params();
347
348 for (unsigned ii = 0; ii < LCs.size(); ii++) {
349 const t_lc::type &tLC = LCs[ii];
350 if (t_lc::includesPhase(tLC)) {
351 for (unsigned iObs = 0; iObs < obsVector.size(); iObs++) {
352 const t_pppSatObs *obs = obsVector[iObs];
353 char sys = obs->prn().system();
354
355 // Check set Slips and Jump Counters
356 // ---------------------------------
357 bool slip = false;
358
359 if (obs->slip()) {
360 LOG << epoTimeStr << " Cycle slip set (obs) " << obs->prn().toString() << endl;
361 slip = true;
362 }
363
364 if (_slips[obs->prn()]._obsSlipCounter != -1 &&
365 _slips[obs->prn()]._obsSlipCounter != obs->slipCounter()) {
366 LOG << epoTimeStr << " Cycle slip set (obsSlipCounter) " << obs->prn().toString() << endl;
367 slip = true;
368 }
369 _slips[obs->prn()]._obsSlipCounter = obs->slipCounter();
370
371 if (_slips[obs->prn()]._biasJumpCounter != -1 &&
372 _slips[obs->prn()]._biasJumpCounter != obs->biasJumpCounter()) {
373 LOG << epoTimeStr << " Cycle slip set (biasJumpCounter) " << obs->prn().toString() << endl;
374 slip = true;
375 }
376 _slips[obs->prn()]._biasJumpCounter = obs->biasJumpCounter();
377
378 // Slip Set
379 // --------
380 if (slip) {
381 resetAmb(obs->prn(), obsVector, tLC);
382 }
383
384 // Check Pre-Fit Residuals
385 /* -----------------------
386 else {
387 ColumnVector AA(params.size());
388 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
389 const t_pppParam* par = params[iPar];
390 AA[iPar] = par->partial(_epoTime, obs, tLC);
391 }
392 double offGps = 0.0;
393 if (sys == 'G' && tLC != t_lc::MW) {
394 offGps = PPP_CLIENT->offGps();
395 }
396 double offGlo = 0.0;
397 if (sys == 'R' && tLC != t_lc::MW) {
398 offGlo = PPP_CLIENT->offGlo();
399 }
400 double offGal = 0.0;
401 if (sys == 'E' && tLC != t_lc::MW) {
402 offGal = PPP_CLIENT->offGal();
403 }
404 double offBds = 0.0;
405 if (sys == 'C' && tLC != t_lc::MW) {
406 offBds = PPP_CLIENT->offBds();
407 }
408 double ll = obs->obsValue(tLC) -offGps - offGlo - offGal - offBds - obs->cmpValue(tLC) - DotProduct(_x0, AA);
409 double vv = DotProduct(AA, _xFlt) - ll;
410
411 if (fabs(vv) > SLIP) {
412 LOG << epoTimeStr << " cycle slip detected " << t_lc::toString(tLC) << ' '
413 << obs->prn().toString() << ' ' << setw(8) << setprecision(4) << vv << endl;
414 resetAmb(obs->prn(), obsVector, tLC);
415 }
416 }*/
417 }
418 }
419 }
420 return success;
421}
422
423// Reset Ambiguity Parameter (cycle slip)
424////////////////////////////////////////////////////////////////////////////
425t_irc t_pppFilter::resetAmb(const t_prn prn, const vector<t_pppSatObs*> &obsVector, t_lc::type lc,
426 SymmetricMatrix *QSav, ColumnVector *xSav) {
427
428 t_irc irc = failure;
429 vector<t_pppParam*>& params = _parlist->params();
430 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
431 t_pppParam *par = params[iPar];
432 if (par->type() == t_pppParam::amb && par->prn() == prn) {
433 int ind = par->indexNew();
434 double eleSat = par->ambEleSat();
435 bncTime firstObsTime;
436 bncTime lastObsTime = par->lastObsTime();
437 (par->firstObsTime().undef()) ?
438 firstObsTime = lastObsTime : firstObsTime = par->firstObsTime();
439 t_lc::type tLC = par->tLC();
440 if (tLC != lc) {continue;}
441 LOG << string(_epoTime) << " RESET " << par->toString() << endl;
442 delete par; par = new t_pppParam(t_pppParam::amb, prn, tLC, &obsVector);
443 par->setIndex(ind);
444 par->setFirstObsTime(firstObsTime);
445 par->setLastObsTime(lastObsTime);
446 par->setAmbEleSat(eleSat);
447 params[iPar] = par;
448 for (unsigned ii = 1; ii <= params.size(); ii++) {
449 _QFlt(ii, ind + 1) = 0.0;
450 if (QSav) {
451 (*QSav)(ii, ind + 1) = 0.0;
452 }
453 }
454 _QFlt(ind + 1, ind + 1) = par->sigma0() * par->sigma0();
455 if (QSav) {
456 (*QSav)(ind + 1, ind + 1) = _QFlt(ind + 1, ind + 1);
457 }
458 _xFlt[ind] = 0.0;
459 if (xSav) {
460 (*xSav)[ind] = _xFlt[ind];
461 }
462 _x0[ind] = par->x0();
463 irc = success;
464 }
465 }
466
467 return irc;
468}
469
470// Compute various DOP Values
471////////////////////////////////////////////////////////////////////////////
472void t_pppFilter::cmpDOP(const vector<t_pppSatObs*> &obsVector) {
473
474 _dop.reset();
475
476 try {
477 const unsigned numPar = 4;
478 Matrix AA(obsVector.size(), numPar);
479 t_pppParam* parX = 0;
480 t_pppParam* parY = 0;
481 t_pppParam* parZ = 0;
482 _numSat = 0;
483 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
484 t_pppSatObs *obs = obsVector[ii];
485 if (obs->isValid() && !obs->outlier()) {
486 ++_numSat;
487 for (unsigned iPar = 0; iPar < numPar; iPar++) {
488 t_pppParam* par = _parlist->params()[iPar];
489 AA[_numSat - 1][iPar] = par->partial(_epoTime, obs, t_lc::c1);
490 if (par->type() == t_pppParam::crdX) {
491 parX = par;
492 }
493 else if (par->type() == t_pppParam::crdY) {
494 parY = par;
495 }
496 else if (par->type() == t_pppParam::crdZ) {
497 parZ = par;
498 }
499 }
500 }
501 }
502 if (_numSat < 4) {
503 return;
504 }
505 AA = AA.Rows(1, _numSat);
506 SymmetricMatrix NN; NN << AA.t() * AA;
507 SymmetricMatrix QQ = NN.i();
508 SymmetricMatrix QQxyz = QQ.SymSubMatrix(1,3);
509
510 ColumnVector xyz(3), neu(3);
511 SymmetricMatrix QQneu(3);
512 const t_pppStation *sta = PPP_CLIENT->staRover();
513 xyz[0] = _xFlt[parX->indexNew()];
514 xyz[1] = _xFlt[parY->indexNew()];
515 xyz[2] = _xFlt[parZ->indexNew()];
516 xyz2neu(sta->ellApr().data(), xyz.data(), neu.data());
517 covariXYZ_NEU(QQxyz, sta->ellApr().data(), QQneu);
518
519 _dop.H = sqrt(QQneu(1, 1) + QQneu(2, 2));
520 _dop.V = sqrt(QQneu(3, 3));
521 _dop.P = sqrt(QQ(1, 1) + QQ(2, 2) + QQ(3, 3));
522 _dop.T = sqrt(QQ(4, 4));
523 _dop.G = sqrt(QQ(1, 1) + QQ(2, 2) + QQ(3, 3) + QQ(4, 4));
524 }
525 catch (...) {
526 }
527}
528
529//
530////////////////////////////////////////////////////////////////////////////
531void t_pppFilter::predictCovCrdPart(const SymmetricMatrix &QFltOld, bool setNeuNoiseToZero) {
532
533 const vector<t_pppParam*>& params = _parlist->params();
534
535 if (params.size() < 3) {
536 return;
537 }
538
539 bool first = (params[0]->indexOld() < 0);
540
541 SymmetricMatrix Qneu(3); Qneu = 0.0;
542 for (unsigned ii = 0; ii < 3; ii++) {
543 const t_pppParam *par = params[ii];
544 if (first) {
545 Qneu[ii][ii] = par->sigma0() * par->sigma0();
546 }
547 else {
548 Qneu[ii][ii] = par->noise() * par->noise();
549 }
550 }
551
552 const t_pppStation *sta = PPP_CLIENT->staRover();
553 SymmetricMatrix Qxyz(3);
554 covariNEU_XYZ(Qneu, sta->ellApr().data(), Qxyz);
555
556 if (first) {
557 _QFlt.SymSubMatrix(1, 3) = Qxyz;
558 }
559 else {
560 double dt = _epoTime - _firstEpoTime;
561 if (dt < OPT->_seedingTime || setNeuNoiseToZero) {
562 _QFlt.SymSubMatrix(1, 3) = QFltOld.SymSubMatrix(1, 3);
563 }
564 else {
565 _QFlt.SymSubMatrix(1, 3) = QFltOld.SymSubMatrix(1, 3) + Qxyz;
566 }
567 }
568}
569
570//
571////////////////////////////////////////////////////////////////////////////
572void t_pppFilter::setStateVectorAndVarCovMatrix(const ColumnVector &xFltOld,
573 const SymmetricMatrix &QFltOld,
574 bool setNeuNoiseToZero) {
575
576 const vector<t_pppParam*>& params = _parlist->params();
577 unsigned nPar = params.size();
578
579 _QFlt.ReSize(nPar); _QFlt = 0.0;
580 _xFlt.ReSize(nPar); _xFlt = 0.0;
581 _x0.ReSize(nPar); _x0 = 0.0;
582
583 for (unsigned ii = 0; ii < nPar; ii++) {
584 t_pppParam *par1 = params[ii];
585 if (QFltOld.size() == 0) {
586 par1->resetIndex();
587 }
588 _x0[ii] = par1->x0();
589 int iOld = par1->indexOld();
590 if (iOld < 0) {
591 _QFlt[ii][ii] = par1->sigma0() * par1->sigma0(); // new parameter
592 } else {
593 _QFlt[ii][ii] = QFltOld[iOld][iOld] + par1->noise() * par1->noise();
594 _xFlt[ii] = xFltOld[iOld];
595 for (unsigned jj = 0; jj < ii; jj++) {
596 t_pppParam *par2 = params[jj];
597 int jOld = par2->indexOld();
598 if (jOld >= 0) {
599 _QFlt[ii][jj] = QFltOld(iOld + 1, jOld + 1);
600 }
601 }
602 }
603 }
604 predictCovCrdPart(QFltOld, setNeuNoiseToZero);
605}
Note: See TracBrowser for help on using the repository browser.