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

Last change on this file since 10319 was 10319, checked in by stuerze, 3 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: 17.2 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) {
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 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 string epoTimeStr = string(_epoTime);
329 const vector<t_pppParam*> &params = _parlist->params();
330
331 for (unsigned ii = 0; ii < LCs.size(); ii++) {
332 const t_lc::type &tLC = LCs[ii];
333 if (t_lc::includesPhase(tLC)) {
334 for (unsigned iObs = 0; iObs < obsVector.size(); iObs++) {
335 const t_pppSatObs *obs = obsVector[iObs];
336 char sys = obs->prn().system();
337
338 // Check set Slips and Jump Counters
339 // ---------------------------------
340 bool slip = false;
341
342 if (obs->slip()) {
343 LOG << epoTimeStr << " Cycle slip set (obs) " << obs->prn().toString() << endl;
344 slip = true;
345 }
346
347 if (_slips[obs->prn()]._obsSlipCounter != -1 &&
348 _slips[obs->prn()]._obsSlipCounter != obs->slipCounter()) {
349 LOG << epoTimeStr << " Cycle slip set (obsSlipCounter) " << obs->prn().toString() << endl;
350 slip = true;
351 }
352 _slips[obs->prn()]._obsSlipCounter = obs->slipCounter();
353
354 if (_slips[obs->prn()]._biasJumpCounter != -1 &&
355 _slips[obs->prn()]._biasJumpCounter != obs->biasJumpCounter()) {
356 LOG << epoTimeStr << " Cycle slip set (biasJumpCounter) " << obs->prn().toString() << endl;
357 slip = true;
358 }
359 _slips[obs->prn()]._biasJumpCounter = obs->biasJumpCounter();
360
361 // Slip Set
362 // --------
363 if (slip) {
364 resetAmb(obs->prn(), obsVector, tLC);
365 }
366
367 // Check Pre-Fit Residuals
368 //=> switched off, because after a millisecond receiver clock jump, a cycle slip is detected for all satellites
369 /* -----------------------
370
371 else {
372 const double SLIP = 20.0;
373 ColumnVector AA(params.size());
374 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
375 const t_pppParam* par = params[iPar];
376 AA[iPar] = par->partial(_epoTime, obs, tLC);
377 }
378 double offGlo = 0;
379 if (sys == 'R' && tLC != t_lc::MW) {
380 offGlo = PPP_CLIENT->offGlo();
381 }
382 double offGal = 0;
383 if (sys == 'E' && tLC != t_lc::MW) {
384 offGal = PPP_CLIENT->offGal();
385 }
386 double offBds = 0;
387 if (sys == 'C' && tLC != t_lc::MW) {
388 offBds = PPP_CLIENT->offBds();
389 }
390 double ll = obs->obsValue(tLC) - offGlo - offGal - offBds - obs->cmpValue(tLC) - DotProduct(_x0, AA);
391 double vv = DotProduct(AA, _xFlt) - ll;
392
393 if (fabs(vv) > SLIP) {
394 LOG << epoTimeStr << " cycle slip detected " << t_lc::toString(tLC) << ' '
395 << obs->prn().toString() << ' ' << setw(8) << setprecision(4) << vv << endl;
396 resetAmb(obs->prn(), obsVector, tLC);
397 }
398 }*/
399 }
400 }
401 }
402
403 return success;
404}
405
406// Reset Ambiguity Parameter (cycle slip)
407////////////////////////////////////////////////////////////////////////////
408t_irc t_pppFilter::resetAmb(const t_prn prn, const vector<t_pppSatObs*> &obsVector, t_lc::type lc,
409 SymmetricMatrix *QSav, ColumnVector *xSav) {
410
411 t_irc irc = failure;
412 vector<t_pppParam*>& params = _parlist->params();
413 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
414 t_pppParam *par = params[iPar];
415 if (par->type() == t_pppParam::amb && par->prn() == prn) {
416 int ind = par->indexNew();
417 double eleSat = par->ambEleSat();
418 bncTime firstObsTime;
419 bncTime lastObsTime = par->lastObsTime();
420 (par->firstObsTime().undef()) ?
421 firstObsTime = lastObsTime : firstObsTime = par->firstObsTime();
422 t_lc::type tLC = par->tLC();
423 if (tLC != lc) {continue;}
424 LOG << string(_epoTime) << " RESET " << par->toString() << endl;
425 delete par; par = new t_pppParam(t_pppParam::amb, prn, tLC, &obsVector);
426 par->setIndex(ind);
427 par->setFirstObsTime(firstObsTime);
428 par->setLastObsTime(lastObsTime);
429 par->setAmbEleSat(eleSat);
430 params[iPar] = par;
431 for (unsigned ii = 1; ii <= params.size(); ii++) {
432 _QFlt(ii, ind + 1) = 0.0;
433 if (QSav) {
434 (*QSav)(ii, ind + 1) = 0.0;
435 }
436 }
437 _QFlt(ind + 1, ind + 1) = par->sigma0() * par->sigma0();
438 if (QSav) {
439 (*QSav)(ind + 1, ind + 1) = _QFlt(ind + 1, ind + 1);
440 }
441 _xFlt[ind] = 0.0;
442 if (xSav) {
443 (*xSav)[ind] = _xFlt[ind];
444 }
445 _x0[ind] = par->x0();
446 irc = success;
447 }
448 }
449
450 return irc;
451}
452
453// Compute various DOP Values
454////////////////////////////////////////////////////////////////////////////
455void t_pppFilter::cmpDOP(const vector<t_pppSatObs*> &obsVector) {
456
457 _dop.reset();
458
459 try {
460 const unsigned numPar = 4;
461 Matrix AA(obsVector.size(), numPar);
462 _numSat = 0;
463 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
464 t_pppSatObs *obs = obsVector[ii];
465 if (obs->isValid() && !obs->outlier()) {
466 ++_numSat;
467 for (unsigned iPar = 0; iPar < numPar; iPar++) {
468 const t_pppParam* par = _parlist->params()[iPar];
469 AA[_numSat - 1][iPar] = par->partial(_epoTime, obs, t_lc::c1);
470 }
471 }
472 }
473 if (_numSat < 4) {
474 return;
475 }
476 AA = AA.Rows(1, _numSat);
477 SymmetricMatrix NN; NN << AA.t() * AA;
478 SymmetricMatrix QQ = NN.i();
479
480 _dop.H = sqrt(QQ(1, 1) + QQ(2, 2));
481 _dop.V = sqrt(QQ(3, 3));
482 _dop.P = sqrt(QQ(1, 1) + QQ(2, 2) + QQ(3, 3));
483 _dop.T = sqrt(QQ(4, 4));
484 _dop.G = sqrt(QQ(1, 1) + QQ(2, 2) + QQ(3, 3) + QQ(4, 4));
485 }
486 catch (...) {
487 }
488}
489
490//
491////////////////////////////////////////////////////////////////////////////
492void t_pppFilter::predictCovCrdPart(const SymmetricMatrix &QFltOld, bool setNeuNoiseToZero) {
493
494 const vector<t_pppParam*>& params = _parlist->params();
495
496 if (params.size() < 3) {
497 return;
498 }
499
500 bool first = (params[0]->indexOld() < 0);
501
502 SymmetricMatrix Qneu(3); Qneu = 0.0;
503 for (unsigned ii = 0; ii < 3; ii++) {
504 const t_pppParam *par = params[ii];
505 if (first) {
506 Qneu[ii][ii] = par->sigma0() * par->sigma0();
507 }
508 else {
509 Qneu[ii][ii] = par->noise() * par->noise();
510 }
511 }
512
513 const t_pppStation *sta = PPP_CLIENT->staRover();
514 SymmetricMatrix Qxyz(3);
515 covariNEU_XYZ(Qneu, sta->ellApr().data(), Qxyz);
516
517 if (first) {
518 _QFlt.SymSubMatrix(1, 3) = Qxyz;
519 }
520 else {
521 double dt = _epoTime - _firstEpoTime;
522 if (dt < OPT->_seedingTime || setNeuNoiseToZero) {
523 _QFlt.SymSubMatrix(1, 3) = QFltOld.SymSubMatrix(1, 3);
524 }
525 else {
526 _QFlt.SymSubMatrix(1, 3) = QFltOld.SymSubMatrix(1, 3) + Qxyz;
527 }
528 }
529}
530
531//
532////////////////////////////////////////////////////////////////////////////
533void t_pppFilter::setStateVectorAndVarCovMatrix(const ColumnVector &xFltOld,
534 const SymmetricMatrix &QFltOld,
535 bool setNeuNoiseToZero) {
536
537 const vector<t_pppParam*>& params = _parlist->params();
538 unsigned nPar = params.size();
539
540 _QFlt.ReSize(nPar); _QFlt = 0.0;
541 _xFlt.ReSize(nPar); _xFlt = 0.0;
542 _x0.ReSize(nPar); _x0 = 0.0;
543
544 for (unsigned ii = 0; ii < nPar; ii++) {
545 t_pppParam *par1 = params[ii];
546 if (QFltOld.size() == 0) {
547 par1->resetIndex();
548 }
549 _x0[ii] = par1->x0();
550 int iOld = par1->indexOld();
551 if (iOld < 0) {
552 _QFlt[ii][ii] = par1->sigma0() * par1->sigma0(); // new parameter
553 } else {
554 _QFlt[ii][ii] = QFltOld[iOld][iOld] + par1->noise() * par1->noise();
555 _xFlt[ii] = xFltOld[iOld];
556 for (unsigned jj = 0; jj < ii; jj++) {
557 t_pppParam *par2 = params[jj];
558 int jOld = par2->indexOld();
559 if (jOld >= 0) {
560 _QFlt[ii][jj] = QFltOld(iOld + 1, jOld + 1);
561 }
562 }
563 }
564 }
565 predictCovCrdPart(QFltOld, setNeuNoiseToZero);
566}
567
Note: See TracBrowser for help on using the repository browser.