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

Last change on this file since 10607 was 10607, checked in by stuerze, 12 months ago

changes regarding PPP

  • 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() {
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); AA = 0.0;
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 if (par->ambResetCandidate()) {
269 resetAmb(obs->prn(), obsVector, maxOutlierLC, &QSav, &xSav);
270 } else {
271 par->setAmbResetCandidate();
272 obs->setOutlier();
273 }
274 }
275 else {
276 obs->setOutlier();
277 }
278 }
279 // Print Residuals
280 // ---------------
281 else {
282 for (unsigned jj = 0; jj < LCs.size(); jj++) {
283 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
284 const t_lc::type tLC = usedTypes[ii];
285 t_pppSatObs *obs = usedObs[ii];
286 if (tLC == LCs[jj]) {
287 obs->setRes(tLC, vv[ii]);
288 LOG << epoTimeStr << " RES " << left << setw(3)
289 << t_lc::toString(tLC) << right << ' '
290 << obs->prn().toString() << ' '
291 << setw(8) << setprecision(4) << vv[ii] << endl;
292 }
293 }
294 }
295 break;
296 }
297 }
298 return success;
299}
300
301// Cycle-Slip Detection
302////////////////////////////////////////////////////////////////////////////
303t_irc t_pppFilter::detectCycleSlips(const vector<t_lc::type> &LCs,
304 const vector<t_pppSatObs*> &obsVector) {
305
306 double SLIP = 100.0;
307 double fac = 1.0;
308 if (_lastEpoTimeOK.valid()) {
309 fac = _epoTime - _lastEpoTimeOK;
310 if (fac > 60.0 || fac < 1.0) {
311 fac = 1.0;
312 }
313 }
314 SLIP *= fac;
315 string epoTimeStr = string(_epoTime);
316 const vector<t_pppParam*> &params = _parlist->params();
317
318 for (unsigned ii = 0; ii < LCs.size(); ii++) {
319 const t_lc::type &tLC = LCs[ii];
320 if (t_lc::includesPhase(tLC)) {
321 for (unsigned iObs = 0; iObs < obsVector.size(); iObs++) {
322 const t_pppSatObs *obs = obsVector[iObs];
323
324 // Check set Slips and Jump Counters
325 // ---------------------------------
326 bool slip = false;
327
328 if (obs->slip()) {
329 LOG << epoTimeStr << " Cycle slip set (obs) " << obs->prn().toString() << endl;
330 slip = true;
331 }
332
333 if (_slips[obs->prn()]._obsSlipCounter != -1 &&
334 _slips[obs->prn()]._obsSlipCounter != obs->slipCounter()) {
335 LOG << epoTimeStr << " Cycle slip set (obsSlipCounter) " << obs->prn().toString() << endl;
336 slip = true;
337 }
338 _slips[obs->prn()]._obsSlipCounter = obs->slipCounter();
339
340 if (_slips[obs->prn()]._biasJumpCounter != -1 &&
341 _slips[obs->prn()]._biasJumpCounter != obs->biasJumpCounter()) {
342 LOG << epoTimeStr << " Cycle slip set (biasJumpCounter) " << obs->prn().toString() << endl;
343 slip = true;
344 }
345 _slips[obs->prn()]._biasJumpCounter = obs->biasJumpCounter();
346
347 // Slip Set
348 // --------
349 if (slip) {
350 resetAmb(obs->prn(), obsVector, tLC);
351 }
352
353 // Check Pre-Fit Residuals
354 // -----------------------
355 else {
356 ColumnVector AA(params.size());
357 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
358 const t_pppParam* par = params[iPar];
359 AA[iPar] = par->partial(_epoTime, obs, tLC);
360 }
361 double ll = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA);
362 double vv = DotProduct(AA, _xFlt) - ll;
363
364 if (fabs(vv) > SLIP) {
365 LOG << epoTimeStr << " cycle slip detected " << t_lc::toString(tLC) << ' '
366 << obs->prn().toString() << ' ' << setw(8) << setprecision(4) << vv << endl;
367 resetAmb(obs->prn(), obsVector, tLC);
368 }
369 }
370 }
371 }
372 }
373 return success;
374}
375
376// Reset Ambiguity Parameter (cycle slip)
377////////////////////////////////////////////////////////////////////////////
378t_irc t_pppFilter::resetAmb(const t_prn prn, const vector<t_pppSatObs*> &obsVector, t_lc::type lc,
379 SymmetricMatrix *QSav, ColumnVector *xSav) {
380
381 t_irc irc = failure;
382 vector<t_pppParam*>& params = _parlist->params();
383 unsigned nPar = params.size();
384 for (unsigned iPar = 0; iPar < nPar; iPar++) {
385 t_pppParam *par = params[iPar];
386 if (par->type() == t_pppParam::amb && par->prn() == prn) {
387 int ind = par->indexNew();
388 double eleSat = par->ambEleSat();
389 bncTime firstObsTime;
390 bncTime lastObsTime = par->lastObsTime();
391 (par->firstObsTime().undef()) ?
392 firstObsTime = lastObsTime : firstObsTime = par->firstObsTime();
393 t_lc::type tLC = par->tLC();
394 if (tLC != lc) {continue;}
395 LOG << string(_epoTime) << " RESET " << par->toString() << endl;
396 delete par; par = new t_pppParam(t_pppParam::amb, prn, tLC, &obsVector);
397 par->setIndex(ind);
398 par->setFirstObsTime(firstObsTime);
399 par->setLastObsTime(lastObsTime);
400 par->setAmbEleSat(eleSat);
401 params[iPar] = par;
402 for (unsigned ii = 0; ii < nPar; ii++) {
403 _QFlt(ii+1, ind+1) = 0.0;
404 if (QSav) {
405 (*QSav)(ii+1, ind+1) = 0.0;
406 }
407 }
408 _QFlt(ind+1,ind+1) = par->sigma0() * par->sigma0();
409 if (QSav) {
410 (*QSav)(ind+1,ind+1) = _QFlt(ind+1,ind+1);
411 }
412 _xFlt[ind] = 0.0;
413 if (xSav) {
414 (*xSav)[ind] = _xFlt[ind];
415 }
416 _x0[ind] = par->x0();
417 irc = success;
418 }
419 }
420
421 return irc;
422}
423
424// Compute various DOP Values
425////////////////////////////////////////////////////////////////////////////
426void t_pppFilter::cmpDOP(const vector<t_pppSatObs*> &obsVector) {
427
428 _dop.reset();
429
430 try {
431 const unsigned numPar = 4;
432 Matrix AA(obsVector.size(), numPar);
433 t_pppParam* parX = 0;
434 t_pppParam* parY = 0;
435 t_pppParam* parZ = 0;
436 _numSat = 0;
437 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
438 t_pppSatObs *obs = obsVector[ii];
439 if (obs->isValid() && !obs->outlier()) {
440 ++_numSat;
441 for (unsigned iPar = 0; iPar < numPar; iPar++) {
442 t_pppParam* par = _parlist->params()[iPar];
443 AA[_numSat - 1][iPar] = par->partial(_epoTime, obs, t_lc::c1);
444 if (par->type() == t_pppParam::crdX) {
445 parX = par;
446 }
447 else if (par->type() == t_pppParam::crdY) {
448 parY = par;
449 }
450 else if (par->type() == t_pppParam::crdZ) {
451 parZ = par;
452 }
453 }
454 }
455 }
456 if (_numSat < 4) {
457 return;
458 }
459 AA = AA.Rows(1, _numSat);
460 SymmetricMatrix NN; NN << AA.t() * AA;
461 SymmetricMatrix QQ = NN.i();
462 SymmetricMatrix QQxyz = QQ.SymSubMatrix(1,3);
463
464 ColumnVector xyz(3), neu(3);
465 SymmetricMatrix QQneu(3);
466 const t_pppStation *sta = PPP_CLIENT->staRover();
467 xyz[0] = _xFlt[parX->indexNew()];
468 xyz[1] = _xFlt[parY->indexNew()];
469 xyz[2] = _xFlt[parZ->indexNew()];
470 xyz2neu(sta->ellApr().data(), xyz.data(), neu.data());
471 covariXYZ_NEU(QQxyz, sta->ellApr().data(), QQneu);
472
473 _dop.H = sqrt(QQneu(1, 1) + QQneu(2, 2));
474 _dop.V = sqrt(QQneu(3, 3));
475 _dop.P = sqrt(QQ(1, 1) + QQ(2, 2) + QQ(3, 3));
476 _dop.T = sqrt(QQ(4, 4));
477 _dop.G = sqrt(QQ(1, 1) + QQ(2, 2) + QQ(3, 3) + QQ(4, 4));
478 }
479 catch (...) {
480 }
481}
482
483//
484////////////////////////////////////////////////////////////////////////////
485void t_pppFilter::predictCovCrdPart(const SymmetricMatrix &QFltOld, bool setNeuNoiseToZero) {
486
487 const vector<t_pppParam*>& params = _parlist->params();
488
489 if (params.size() < 3) {
490 return;
491 }
492
493 bool first = (params[0]->indexOld() < 0);
494
495 SymmetricMatrix Qneu(3); Qneu = 0.0;
496 for (unsigned ii = 0; ii < 3; ii++) {
497 const t_pppParam *par = params[ii];
498 if (first) {
499 Qneu[ii][ii] = par->sigma0() * par->sigma0();
500 }
501 else {
502 Qneu[ii][ii] = par->noise() * par->noise();
503 }
504 }
505
506 const t_pppStation *sta = PPP_CLIENT->staRover();
507 SymmetricMatrix Qxyz(3);
508 covariNEU_XYZ(Qneu, sta->ellApr().data(), Qxyz);
509
510 if (first) {
511 _QFlt.SymSubMatrix(1, 3) = Qxyz;
512 }
513 else {
514 double dt = _epoTime - _firstEpoTime;
515 if (dt < OPT->_seedingTime || setNeuNoiseToZero) {
516 _QFlt.SymSubMatrix(1, 3) = QFltOld.SymSubMatrix(1, 3);
517 }
518 else {
519 _QFlt.SymSubMatrix(1, 3) = QFltOld.SymSubMatrix(1, 3) + Qxyz;
520 }
521 }
522}
523
524//
525////////////////////////////////////////////////////////////////////////////
526void t_pppFilter::setStateVectorAndVarCovMatrix(const ColumnVector &xFltOld,
527 const SymmetricMatrix &QFltOld,
528 bool setNeuNoiseToZero) {
529
530 const vector<t_pppParam*>& params = _parlist->params();
531 unsigned nPar = params.size();
532
533 _QFlt.ReSize(nPar); _QFlt = 0.0;
534 _xFlt.ReSize(nPar); _xFlt = 0.0;
535 _x0.ReSize(nPar); _x0 = 0.0;
536
537 for (unsigned ii = 0; ii < nPar; ii++) {
538 t_pppParam *par1 = params[ii];
539 if (QFltOld.size() == 0) {
540 par1->resetIndex();
541 }
542 _x0[ii] = par1->x0();
543 int iOld = par1->indexOld();
544 if (iOld < 0) {
545 _QFlt[ii][ii] = par1->sigma0() * par1->sigma0(); // new parameter
546 } else {
547 _QFlt[ii][ii] = QFltOld[iOld][iOld] + par1->noise() * par1->noise();
548 _xFlt[ii] = xFltOld[iOld];
549 for (unsigned jj = 0; jj < ii; jj++) {
550 t_pppParam *par2 = params[jj];
551 int jOld = par2->indexOld();
552 if (jOld >= 0) {
553 _QFlt[ii][jj] = QFltOld(iOld + 1, jOld + 1);
554 }
555 }
556 }
557 }
558 predictCovCrdPart(QFltOld, setNeuNoiseToZero);
559}
Note: See TracBrowser for help on using the repository browser.