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

Last change on this file since 10256 was 10256, checked in by stuerze, 5 months ago

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.3 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;
188 if (sys == 'R' && tLC != t_lc::MW) {
189 offGlo = PPP_CLIENT->offGlo();
190 }
191 double offGal = 0;
192 if (sys == 'E' && tLC != t_lc::MW) {
193 offGal = PPP_CLIENT->offGal();
194 }
195 double offBds = 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 < 2) {
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;
232 if (sys == 'R' && tLC != t_lc::MW) {
233 offGlo = PPP_CLIENT->offGlo();
234 }
235 double offGal = 0;
236 if (sys == 'E' && tLC != t_lc::MW) {
237 offGal = PPP_CLIENT->offGal();
238 }
239 double offBds = 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// if (par->ambResetCandidate()) {
296 resetAmb(obs->prn(), obsVector, maxOutlierLC, &QSav, &xSav);
297// }
298// else {
299// par->setAmbResetCandidate();
300// obs->setOutlier();
301// }
302 }
303 else {
304 obs->setOutlier();
305 }
306 }
307 // Print Residuals
308 // ---------------
309 else {
310 for (unsigned jj = 0; jj < LCs.size(); jj++) {
311 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
312 const t_lc::type tLC = usedTypes[ii];
313 t_pppSatObs *obs = usedObs[ii];
314 if (tLC == LCs[jj]) {
315 obs->setRes(tLC, vv[ii]);
316 LOG << epoTimeStr << " RES " << left << setw(3)
317 << t_lc::toString(tLC) << right << ' '
318 << obs->prn().toString() << ' '
319 << setw(8) << setprecision(4) << vv[ii] << endl;
320 }
321 }
322 }
323 break;
324 }
325 }
326 return success;
327}
328
329// Cycle-Slip Detection
330////////////////////////////////////////////////////////////////////////////
331t_irc t_pppFilter::detectCycleSlips(const vector<t_lc::type> &LCs,
332 const vector<t_pppSatObs*> &obsVector) {
333
334 const double SLIP = 20.0;
335 string epoTimeStr = string(_epoTime);
336 const vector<t_pppParam*> &params = _parlist->params();
337
338 for (unsigned ii = 0; ii < LCs.size(); ii++) {
339 const t_lc::type &tLC = LCs[ii];
340 if (t_lc::includesPhase(tLC)) {
341 for (unsigned iObs = 0; iObs < obsVector.size(); iObs++) {
342 const t_pppSatObs *obs = obsVector[iObs];
343 char sys = obs->prn().system();
344
345 // Check set Slips and Jump Counters
346 // ---------------------------------
347 bool slip = false;
348
349 if (obs->slip()) {
350 LOG << epoTimeStr << "cycle slip set (obs) " << obs->prn().toString() << endl;
351 slip = true;
352 }
353
354 if (_slips[obs->prn()]._obsSlipCounter != -1 &&
355 _slips[obs->prn()]._obsSlipCounter != obs->slipCounter()) {
356 LOG << epoTimeStr << "cycle slip set (obsSlipCounter) " << obs->prn().toString() << endl;
357 slip = true;
358 }
359 _slips[obs->prn()]._obsSlipCounter = obs->slipCounter();
360
361 if (_slips[obs->prn()]._biasJumpCounter != -1 &&
362 _slips[obs->prn()]._biasJumpCounter != obs->biasJumpCounter()) {
363 LOG << epoTimeStr << "cycle slip set (biasJumpCounter) " << obs->prn().toString() << endl;
364 slip = true;
365 }
366 _slips[obs->prn()]._biasJumpCounter = obs->biasJumpCounter();
367
368 // Slip Set
369 // --------
370 if (slip) {
371 resetAmb(obs->prn(), obsVector, tLC);
372 }
373
374 // Check Pre-Fit Residuals => switched off, because after a millisecond receiver clock jump, a cycle slip is detected for all satellites
375 /* -----------------------
376
377 else {
378 ColumnVector AA(params.size());
379 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
380 const t_pppParam* par = params[iPar];
381 AA[iPar] = par->partial(_epoTime, obs, tLC);
382 }
383 double offGlo = 0;
384 if (sys == 'R' && tLC != t_lc::MW) {
385 offGlo = PPP_CLIENT->offGlo();
386 }
387 double offGal = 0;
388 if (sys == 'E' && tLC != t_lc::MW) {
389 offGal = PPP_CLIENT->offGal();
390 }
391 double offBds = 0;
392 if (sys == 'C' && tLC != t_lc::MW) {
393 offBds = PPP_CLIENT->offBds();
394 }
395 double ll = obs->obsValue(tLC) - offGlo - offGal - offBds - obs->cmpValue(tLC) - DotProduct(_x0, AA);
396 double vv = DotProduct(AA, _xFlt) - ll;
397
398 if (fabs(vv) > SLIP) {
399 LOG << epoTimeStr << " cycle slip detected " << t_lc::toString(tLC) << ' '
400 << obs->prn().toString() << ' ' << setw(8) << setprecision(4) << vv << endl;
401 resetAmb(obs->prn(), obsVector, tLC);
402 }
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 _numSat = 0;
468 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
469 t_pppSatObs *obs = obsVector[ii];
470 if (obs->isValid() && !obs->outlier()) {
471 ++_numSat;
472 for (unsigned iPar = 0; iPar < numPar; iPar++) {
473 const t_pppParam* par = _parlist->params()[iPar];
474 AA[_numSat - 1][iPar] = par->partial(_epoTime, obs, t_lc::c1);
475 }
476 }
477 }
478 if (_numSat < 4) {
479 return;
480 }
481 AA = AA.Rows(1, _numSat);
482 SymmetricMatrix NN; NN << AA.t() * AA;
483 SymmetricMatrix QQ = NN.i();
484
485 _dop.H = sqrt(QQ(1, 1) + QQ(2, 2));
486 _dop.V = sqrt(QQ(3, 3));
487 _dop.P = sqrt(QQ(1, 1) + QQ(2, 2) + QQ(3, 3));
488 _dop.T = sqrt(QQ(4, 4));
489 _dop.G = sqrt(QQ(1, 1) + QQ(2, 2) + QQ(3, 3) + QQ(4, 4));
490 }
491 catch (...) {
492 }
493}
494
495//
496////////////////////////////////////////////////////////////////////////////
497void t_pppFilter::predictCovCrdPart(const SymmetricMatrix &QFltOld, bool setNeuNoiseToZero) {
498
499 const vector<t_pppParam*>& params = _parlist->params();
500
501 if (params.size() < 3) {
502 return;
503 }
504
505 bool first = (params[0]->indexOld() < 0);
506
507 SymmetricMatrix Qneu(3); Qneu = 0.0;
508 for (unsigned ii = 0; ii < 3; ii++) {
509 const t_pppParam *par = params[ii];
510 if (first) {
511 Qneu[ii][ii] = par->sigma0() * par->sigma0();
512 }
513 else {
514 Qneu[ii][ii] = par->noise() * par->noise();
515 }
516 }
517
518 const t_pppStation *sta = PPP_CLIENT->staRover();
519 SymmetricMatrix Qxyz(3);
520 covariNEU_XYZ(Qneu, sta->ellApr().data(), Qxyz);
521
522 if (first) {
523 _QFlt.SymSubMatrix(1, 3) = Qxyz;
524 }
525 else {
526 double dt = _epoTime - _firstEpoTime;
527 if (dt < OPT->_seedingTime || setNeuNoiseToZero) {
528 _QFlt.SymSubMatrix(1, 3) = QFltOld.SymSubMatrix(1, 3);
529 }
530 else {
531 _QFlt.SymSubMatrix(1, 3) = QFltOld.SymSubMatrix(1, 3) + Qxyz;
532 }
533 }
534}
535
536//
537////////////////////////////////////////////////////////////////////////////
538void t_pppFilter::setStateVectorAndVarCovMatrix(const ColumnVector &xFltOld,
539 const SymmetricMatrix &QFltOld,
540 bool setNeuNoiseToZero) {
541
542 const vector<t_pppParam*>& params = _parlist->params();
543 unsigned nPar = params.size();
544
545 _QFlt.ReSize(nPar); _QFlt = 0.0;
546 _xFlt.ReSize(nPar); _xFlt = 0.0;
547 _x0.ReSize(nPar); _x0 = 0.0;
548
549 for (unsigned ii = 0; ii < nPar; ii++) {
550 t_pppParam *par1 = params[ii];
551 if (QFltOld.size() == 0) {
552 par1->resetIndex();
553 }
554 _x0[ii] = par1->x0();
555 int iOld = par1->indexOld();
556 if (iOld < 0) {
557 _QFlt[ii][ii] = par1->sigma0() * par1->sigma0(); // new parameter
558 } else {
559 _QFlt[ii][ii] = QFltOld[iOld][iOld] + par1->noise() * par1->noise();
560 _xFlt[ii] = xFltOld[iOld];
561 for (unsigned jj = 0; jj < ii; jj++) {
562 t_pppParam *par2 = params[jj];
563 int jOld = par2->indexOld();
564 if (jOld >= 0) {
565 _QFlt[ii][jj] = QFltOld(iOld + 1, jOld + 1);
566 }
567 }
568 }
569 }
570 predictCovCrdPart(QFltOld, setNeuNoiseToZero);
571}
572
Note: See TracBrowser for help on using the repository browser.