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

Last change on this file since 8905 was 8905, checked in by stuerze, 4 years ago

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