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

Last change on this file was 11051, checked in by stuerze, 6 weeks ago

filter reset after exception

  • 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.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 <sstream>
19#include <iomanip>
20#include <cmath>
21#include <newmat.h>
22#include <newmatio.h>
23#include <newmatap.h>
24
25#include "pppFilter.h"
26#include "bncutils.h"
27#include "pppParlist.h"
28#include "pppObsPool.h"
29#include "pppStation.h"
30#include "pppClient.h"
31#include "ambres.h"
32
33using namespace BNC_PPP;
34using namespace std;
35
36// Constructor
37////////////////////////////////////////////////////////////////////////////
38t_pppFilter::t_pppFilter() {
39 _parlist = 0;
40}
41
42// Destructor
43////////////////////////////////////////////////////////////////////////////
44t_pppFilter::~t_pppFilter() {
45 delete _parlist;
46}
47
48// Process Single Epoch
49////////////////////////////////////////////////////////////////////////////
50t_irc t_pppFilter::processEpoch(t_pppObsPool* obsPool) {
51
52 _numSat = 0;
53 const double maxSolGap = 60.0;
54 bool setNeuNoiseToZero = false;
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 // 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 (unsigned iSys = 0; iSys < OPT->systems().size(); iSys++) {
95 char sys = OPT->systems()[iSys];
96 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 // Dillution of Precision
112 // ----------------------
113 cmpDOP(allObs);
114
115 // Ambiguity Resolution
116 // --------------------
117 if (OPT->ambRes()) {
118 AmbRes ambRes;
119 ostringstream msg;
120 ColumnVector xFix = _xFlt;
121 SymmetricMatrix QFix = _QFlt;
122 _fixRatio = 0.0;
123 ambRes.run(_epoTime, _parlist->params(), QFix, xFix, _fixRatio, msg);
124 LOG << msg.str();
125 _parlist->printResult(_epoTime, QFix, xFix, _fixRatio);
126 }
127
128 // Float Solution
129 // --------------
130 else {
131 _parlist->printResult(_epoTime, _QFlt, _xFlt);
132 }
133
134 _lastEpoTimeOK = _epoTime; // remember time of last successful epoch processing
135
136 return success;
137}
138
139// Process Selected LCs
140////////////////////////////////////////////////////////////////////////////
141t_irc t_pppFilter::processSystem(const vector<t_lc>& LCs,
142 const vector<t_pppSatObs*> &obsVector,
143 bool pseudoObsIonoAvailable) {
144 LOG.setf(ios::fixed);
145
146 // Detect Cycle Slips
147 // ------------------
148 if (detectCycleSlips(LCs, obsVector) != success) {
149 return failure;
150 }
151
152 ColumnVector xSav = _xFlt;
153 SymmetricMatrix QSav = _QFlt;
154 string epoTimeStr = string(_epoTime);
155 const vector<t_pppParam*> &params = _parlist->params();
156 unsigned nPar = _parlist->nPar();
157
158 unsigned usedLCs = LCs.size();
159 if (OPT->_pseudoObsIono && !pseudoObsIonoAvailable) {
160 usedLCs -= 1; // GIM not used
161 }
162 // max Obs
163 unsigned maxObs = obsVector.size() * usedLCs;
164
165 // Outlier Detection Loop
166 // ----------------------
167 for (unsigned iOutlier = 0; iOutlier < maxObs; iOutlier++) {
168
169 if (iOutlier > 0) {
170 _xFlt = xSav;
171 _QFlt = QSav;
172 }
173
174 // First-Design Matrix, Terms Observed-Computed, Weight Matrix
175 // -----------------------------------------------------------
176 Matrix AA(maxObs, nPar); AA = 0.0;
177 ColumnVector ll(maxObs); ll = 0.0;
178 DiagonalMatrix PP(maxObs); PP = 0.0;
179
180 int iObs = -1;
181 vector<t_pppSatObs*> usedObs;
182 vector<t_lc> usedTypes;
183
184 // Real Observations
185 // =================
186 int nSat = 0;
187 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
188 t_pppSatObs *obs = obsVector[ii];
189 if (iOutlier == 0) {
190 obs->resetOutlier();
191 }
192 if (!obs->outlier()) {
193 nSat++;
194 for (unsigned jj = 0; jj < usedLCs; jj++) {
195 const t_lc LC = LCs[jj];
196 if (LC._type == t_lc::GIM) {
197 continue;
198 }
199 if (LC._frq1 == t_frequency::G5 && !obs->isValid(LC)) {
200 continue;
201 }
202 ++iObs;
203 usedObs.push_back(obs);
204 usedTypes.push_back(LC);
205 for (unsigned iPar = 0; iPar < nPar; iPar++) {
206 const t_pppParam *par = params[iPar];
207 AA[iObs][iPar] = par->partial(_epoTime, obs, LC);
208 }
209
210 ll[iObs] = obs->obsValue(LC) - obs->cmpValue(LC) - DotProduct(_x0, AA.Row(iObs + 1));
211 PP[iObs] = 1.0 / (obs->sigma(LC) * obs->sigma(LC));
212 }
213 }
214 }
215
216 // Check number of observations
217 // ----------------------------
218 if (!nSat) {
219 return failure;
220 }
221
222 // Pseudo Obs Iono
223 // ================
224 if (OPT->_pseudoObsIono && pseudoObsIonoAvailable) {
225 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
226 t_pppSatObs *obs = obsVector[ii];
227 if (!obs->outlier()) {
228 for (unsigned jj = 0; jj < usedLCs; jj++) {
229 const t_lc LC = LCs[jj];
230 if (LC._type == t_lc::GIM) {
231 if (!obs->isValid(LC)) continue;
232 ++iObs;
233 } else {
234 continue;
235 }
236 usedObs.push_back(obs);
237 usedTypes.push_back(LC);
238 for (unsigned iPar = 0; iPar < nPar; iPar++) {
239 const t_pppParam *par = params[iPar];
240 AA[iObs][iPar] = par->partial(_epoTime, obs, LC);
241 }
242 ll[iObs] = obs->obsValue(LC) - obs->cmpValue(LC) - DotProduct(_x0, AA.Row(iObs + 1));
243 PP[iObs] = 1.0 / (obs->sigma(LC) * obs->sigma(LC));
244 }
245 }
246 }
247 }
248
249 // Truncate matrices
250 // -----------------
251 AA = AA.Rows(1, iObs + 1);
252 ll = ll.Rows(1, iObs + 1);
253 PP = PP.SymSubMatrix(1, iObs + 1);
254
255 // Kalman update step
256 // ------------------
257 try {
258 kalman(AA, ll, PP, _QFlt, _xFlt);
259 }
260 catch (Exception&) {
261 logNonPosDef(_QFlt, params);
262 throw;
263 }
264
265 // Check Residuals
266 // ---------------
267 ColumnVector vv = AA * _xFlt - ll;
268 double maxOutlier = 0.0;
269 int maxOutlierIndex = -1;
270 t_lc maxOutlierLC;
271 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
272 const t_lc LC = usedTypes[ii];
273 if (LC._type == t_lc::GIM) continue; // GIM is a soft constraint; never reject as outlier
274 double res = fabs(vv[ii]);
275 if (res > usedObs[ii]->maxRes(LC)) {
276 if (res > fabs(maxOutlier)) {
277 maxOutlier = vv[ii];
278 maxOutlierIndex = ii;
279 maxOutlierLC = LC;
280 }
281 }
282 }
283
284 // Mark outlier or break outlier detection loop
285 // --------------------------------------------
286 if (maxOutlierIndex > -1) {
287 t_pppSatObs *obs = usedObs[maxOutlierIndex];
288 t_pppParam *par = 0;
289 LOG << epoTimeStr << " Outlier " << maxOutlierLC.toString() << ' '
290 << obs->prn().toString() << ' ' << setw(8) << setprecision(4)
291 << maxOutlier << endl;
292 for (unsigned iPar = 0; iPar < nPar; iPar++) {
293 t_pppParam* hlp = params[iPar];
294 if (hlp->type() == t_pppParam::amb &&
295 hlp->prn() == obs->prn() &&
296 hlp->LC() == usedTypes[maxOutlierIndex]) {
297 par = hlp;
298 }
299 }
300 if (par) {
301 resetAmb(obs->prn(), obsVector, maxOutlierLC, &QSav, &xSav);
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 LC = usedTypes[ii];
313 t_pppSatObs* obs = usedObs[ii];
314 if (LC == LCs[jj]) {
315 obs->setRes(LC, vv[ii]);
316 LOG << epoTimeStr << " RES " << left << setw(3)
317 << LC.toString() << 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// Diagnose a Variance-Covariance Matrix that failed Cholesky decomposition
330////////////////////////////////////////////////////////////////////////////
331void t_pppFilter::logNonPosDef(const SymmetricMatrix& QQ,
332 const vector<t_pppParam*>& params) const {
333
334 string epoTimeStr = string(_epoTime);
335 int nn = QQ.Nrows();
336
337 for (int ii = 1; ii <= nn; ii++) {
338 double qq = QQ(ii,ii);
339 if (!(qq > 0.0)) {
340 LOG << epoTimeStr << " NotPosDef diag " << params[ii-1]->toString()
341 << " Q = " << scientific << qq << fixed << endl;
342 }
343 }
344
345 // Cholesky pivots: first index where the matrix stops being positive definite
346 // ---------------------------------------------------------------------------
347 Matrix LL(nn, nn); LL = 0.0;
348 for (int jj = 1; jj <= nn; jj++) {
349 double sum = QQ(jj,jj);
350 for (int kk = 1; kk < jj; kk++) {
351 sum -= LL(jj,kk) * LL(jj,kk);
352 }
353 if (!(sum > 0.0)) {
354 const t_pppParam* par = params[jj-1];
355 LOG << epoTimeStr << " NotPosDef pivot " << jj << " " << par->toString()
356 << " Q = " << scientific << QQ(jj,jj) << " pivot = " << sum
357 << fixed << " indexOld = " << par->indexOld() << endl;
358 for (int kk = 1; kk < jj; kk++) {
359 double rho = QQ(jj,kk) / sqrt(QQ(jj,jj) * QQ(kk,kk));
360 if (fabs(rho) > 0.999) {
361 LOG << epoTimeStr << " NotPosDef corr " << params[kk-1]->toString()
362 << ' ' << setprecision(9) << rho << endl;
363 }
364 }
365 return;
366 }
367 LL(jj,jj) = sqrt(sum);
368 for (int ii = jj + 1; ii <= nn; ii++) {
369 double hlp = QQ(ii,jj);
370 for (int kk = 1; kk < jj; kk++) {
371 hlp -= LL(ii,kk) * LL(jj,kk);
372 }
373 LL(ii,jj) = hlp / LL(jj,jj);
374 }
375 }
376}
377
378// Cycle-Slip Detection
379////////////////////////////////////////////////////////////////////////////
380t_irc t_pppFilter::detectCycleSlips(const vector<t_lc>& LCs,
381 const vector<t_pppSatObs*> &obsVector) {
382
383 double SLIP = 150.0;
384 double fac = 1.0;
385 if (_lastEpoTimeOK.valid()) {
386 fac = _epoTime - _lastEpoTimeOK;
387 if (fac > 60.0 || fac < 1.0) {
388 fac = 1.0;
389 }
390 }
391 SLIP *= fac;
392 string epoTimeStr = string(_epoTime);
393
394 for (unsigned ii = 0; ii < LCs.size(); ii++) {
395 const t_lc& LC = LCs[ii];
396 if (LC.includesPhase()) {
397 for (unsigned iObs = 0; iObs < obsVector.size(); iObs++) {
398 const t_pppSatObs *obs = obsVector[iObs];
399
400 // Check set Slips and Jump Counters
401 // ---------------------------------
402 bool slip = false;
403
404 if (obs->slip()) {
405 LOG << epoTimeStr << " Cycle slip set (obs) " << obs->prn().toString() << endl;
406 slip = true;
407 }
408
409 if (_slips[obs->prn()]._obsSlipCounter != -1 &&
410 _slips[obs->prn()]._obsSlipCounter != obs->slipCounter()) {
411 LOG << epoTimeStr << " Cycle slip set (obsSlipCounter) " << obs->prn().toString() << endl;
412 slip = true;
413 }
414 _slips[obs->prn()]._obsSlipCounter = obs->slipCounter();
415
416 if (_slips[obs->prn()]._biasJumpCounter != -1 &&
417 _slips[obs->prn()]._biasJumpCounter != obs->biasJumpCounter()) {
418 LOG << epoTimeStr << " Cycle slip set (biasJumpCounter) " << obs->prn().toString() << endl;
419 slip = true;
420 }
421 _slips[obs->prn()]._biasJumpCounter = obs->biasJumpCounter();
422
423 // Slip Set
424 // --------
425 if (slip) {
426 resetAmb(obs->prn(), obsVector, t_lc());
427 }
428 }
429 }
430 }
431 return success;
432}
433
434// Reset Ambiguity Parameter (cycle slip)
435////////////////////////////////////////////////////////////////////////////
436t_irc t_pppFilter::resetAmb(const t_prn prn, const vector<t_pppSatObs*> &obsVector, t_lc lc,
437 SymmetricMatrix *QSav, ColumnVector *xSav) {
438
439 t_irc irc = failure;
440 vector<t_pppParam*>& params = _parlist->params();
441 unsigned nPar = params.size();
442 for (unsigned iPar = 0; iPar < nPar; iPar++) {
443 t_pppParam *par = params[iPar];
444 if (par->type() == t_pppParam::amb && par->prn() == prn) {
445 int ind = par->indexNew();
446 double eleSat = par->ambEleSat();
447 bncTime firstObsTime;
448 bncTime lastObsTime = par->lastObsTime();
449 (par->firstObsTime().undef()) ?
450 firstObsTime = lastObsTime : firstObsTime = par->firstObsTime();
451 t_lc LC = par->LC();
452 if (lc.valid() && LC != lc) {continue;}
453 LOG << string(_epoTime) << " RESET " << par->toString() << endl;
454 delete par; par = new t_pppParam(t_pppParam::amb, prn, LC, &obsVector);
455 par->setIndex(ind);
456 par->setFirstObsTime(firstObsTime);
457 par->setLastObsTime(lastObsTime);
458 par->setAmbEleSat(eleSat);
459 params[iPar] = par;
460 for (unsigned ii = 0; ii < nPar; ii++) {
461 _QFlt(ii+1, ind+1) = 0.0;
462 if (QSav) {
463 (*QSav)(ii+1, ind+1) = 0.0;
464 }
465 }
466 _QFlt(ind+1,ind+1) = par->sigma0() * par->sigma0();
467 if (QSav) {
468 (*QSav)(ind+1,ind+1) = _QFlt(ind+1,ind+1);
469 }
470 _xFlt[ind] = 0.0;
471 if (xSav) {
472 (*xSav)[ind] = _xFlt[ind];
473 }
474 _x0[ind] = par->x0();
475 irc = success;
476 }
477 }
478
479 return irc;
480}
481
482// Compute various DOP Values
483////////////////////////////////////////////////////////////////////////////
484void t_pppFilter::cmpDOP(const vector<t_pppSatObs*> &obsVector) {
485
486 _dop.reset();
487
488 try {
489 const unsigned numPar = 4;
490 Matrix AA(obsVector.size(), numPar);
491 t_pppParam* parX = 0;
492 t_pppParam* parY = 0;
493 t_pppParam* parZ = 0;
494 _numSat = 0;
495 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
496 t_pppSatObs *obs = obsVector[ii];
497 if (obs->isValid() && !obs->outlier()) {
498 ++_numSat;
499 for (unsigned iPar = 0; iPar < numPar; iPar++) {
500 t_pppParam* par = _parlist->params()[iPar];
501 AA[_numSat - 1][iPar] = par->partial(_epoTime, obs, obs->rangeLC());
502 if (par->type() == t_pppParam::crdX) {
503 parX = par;
504 }
505 else if (par->type() == t_pppParam::crdY) {
506 parY = par;
507 }
508 else if (par->type() == t_pppParam::crdZ) {
509 parZ = par;
510 }
511 }
512 }
513 }
514 if (_numSat < 4) {
515 return;
516 }
517 AA = AA.Rows(1, _numSat);
518 SymmetricMatrix NN; NN << AA.t() * AA;
519 SymmetricMatrix QQ = NN.i();
520 SymmetricMatrix QQxyz = QQ.SymSubMatrix(1,3);
521
522 ColumnVector xyz(3), neu(3);
523 SymmetricMatrix QQneu(3);
524 const t_pppStation *sta = PPP_CLIENT->staRover();
525 xyz[0] = _xFlt[parX->indexNew()];
526 xyz[1] = _xFlt[parY->indexNew()];
527 xyz[2] = _xFlt[parZ->indexNew()];
528 xyz2neu(sta->ellApr().data(), xyz.data(), neu.data());
529 covariXYZ_NEU(QQxyz, sta->ellApr().data(), QQneu);
530
531 _dop.H = sqrt(QQneu(1, 1) + QQneu(2, 2));
532 _dop.V = sqrt(QQneu(3, 3));
533 _dop.P = sqrt(QQ(1, 1) + QQ(2, 2) + QQ(3, 3));
534 _dop.T = sqrt(QQ(4, 4));
535 _dop.G = sqrt(QQ(1, 1) + QQ(2, 2) + QQ(3, 3) + QQ(4, 4));
536 }
537 catch (...) {
538 }
539}
540
541//
542////////////////////////////////////////////////////////////////////////////
543void t_pppFilter::predictCovCrdPart(const SymmetricMatrix &QFltOld, bool setNeuNoiseToZero) {
544
545 const vector<t_pppParam*>& params = _parlist->params();
546
547 if (params.size() < 3) {
548 return;
549 }
550
551 bool first = (params[0]->indexOld() < 0);
552
553 SymmetricMatrix Qneu(3); Qneu = 0.0;
554 for (unsigned ii = 0; ii < 3; ii++) {
555 const t_pppParam *par = params[ii];
556 if (first) {
557 Qneu[ii][ii] = par->sigma0() * par->sigma0();
558 }
559 else {
560 Qneu[ii][ii] = par->noise() * par->noise();
561 }
562 }
563
564 const t_pppStation *sta = PPP_CLIENT->staRover();
565 SymmetricMatrix Qxyz(3);
566 covariNEU_XYZ(Qneu, sta->ellApr().data(), Qxyz);
567
568 if (first) {
569 _QFlt.SymSubMatrix(1, 3) = Qxyz;
570 }
571 else {
572 double dt = _epoTime - _firstEpoTime;
573 if (dt < OPT->_seedingTime || setNeuNoiseToZero) {
574 _QFlt.SymSubMatrix(1, 3) = QFltOld.SymSubMatrix(1, 3);
575 }
576 else {
577 _QFlt.SymSubMatrix(1, 3) = QFltOld.SymSubMatrix(1, 3) + Qxyz;
578 }
579 }
580}
581
582//
583////////////////////////////////////////////////////////////////////////////
584void t_pppFilter::setStateVectorAndVarCovMatrix(const ColumnVector &xFltOld,
585 const SymmetricMatrix &QFltOld,
586 bool setNeuNoiseToZero) {
587
588 const vector<t_pppParam*>& params = _parlist->params();
589 unsigned nPar = params.size();
590
591 _QFlt.ReSize(nPar); _QFlt = 0.0;
592 _xFlt.ReSize(nPar); _xFlt = 0.0;
593 _x0.ReSize(nPar); _x0 = 0.0;
594
595 for (unsigned ii = 0; ii < nPar; ii++) {
596 t_pppParam *par1 = params[ii];
597 if (QFltOld.size() == 0) {
598 par1->resetIndex();
599 }
600 _x0[ii] = par1->x0();
601 int iOld = par1->indexOld();
602 if (iOld < 0) {
603 _QFlt[ii][ii] = par1->sigma0() * par1->sigma0(); // new parameter
604 } else {
605 _QFlt[ii][ii] = QFltOld[iOld][iOld] + par1->noise() * par1->noise();
606 _xFlt[ii] = xFltOld[iOld];
607 for (unsigned jj = 0; jj < ii; jj++) {
608 t_pppParam *par2 = params[jj];
609 int jOld = par2->indexOld();
610 if (jOld >= 0) {
611 _QFlt[ii][jj] = QFltOld(iOld + 1, jOld + 1);
612 }
613 }
614 }
615 }
616 predictCovCrdPart(QFltOld, setNeuNoiseToZero);
617}
Note: See TracBrowser for help on using the repository browser.