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

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