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

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