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

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