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

Last change on this file was 10411, checked in by stuerze, 4 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 }
363 }
364 }
365 }
366 }
367 return success;
368}
369
370// Reset Ambiguity Parameter (cycle slip)
371////////////////////////////////////////////////////////////////////////////
372t_irc t_pppFilter::resetAmb(const t_prn prn, const vector<t_pppSatObs*> &obsVector, t_lc::type lc,
373 SymmetricMatrix *QSav, ColumnVector *xSav) {
374
375 t_irc irc = failure;
376 vector<t_pppParam*>& params = _parlist->params();
377 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
378 t_pppParam *par = params[iPar];
379 if (par->type() == t_pppParam::amb && par->prn() == prn) {
380 int ind = par->indexNew();
381 double eleSat = par->ambEleSat();
382 bncTime firstObsTime;
383 bncTime lastObsTime = par->lastObsTime();
384 (par->firstObsTime().undef()) ?
385 firstObsTime = lastObsTime : firstObsTime = par->firstObsTime();
386 t_lc::type tLC = par->tLC();
387 if (tLC != lc) {continue;}
388 LOG << string(_epoTime) << " RESET " << par->toString() << endl;
389 delete par; par = new t_pppParam(t_pppParam::amb, prn, tLC, &obsVector);
390 par->setIndex(ind);
391 par->setFirstObsTime(firstObsTime);
392 par->setLastObsTime(lastObsTime);
393 par->setAmbEleSat(eleSat);
394 params[iPar] = par;
395 for (unsigned ii = 1; ii <= params.size(); ii++) {
396 _QFlt(ii, ind + 1) = 0.0;
397 if (QSav) {
398 (*QSav)(ii, ind + 1) = 0.0;
399 }
400 }
401 _QFlt(ind + 1, ind + 1) = par->sigma0() * par->sigma0();
402 if (QSav) {
403 (*QSav)(ind + 1, ind + 1) = _QFlt(ind + 1, ind + 1);
404 }
405 _xFlt[ind] = 0.0;
406 if (xSav) {
407 (*xSav)[ind] = _xFlt[ind];
408 }
409 _x0[ind] = par->x0();
410 irc = success;
411 }
412 }
413
414 return irc;
415}
416
417// Compute various DOP Values
418////////////////////////////////////////////////////////////////////////////
419void t_pppFilter::cmpDOP(const vector<t_pppSatObs*> &obsVector) {
420
421 _dop.reset();
422
423 try {
424 const unsigned numPar = 4;
425 Matrix AA(obsVector.size(), numPar);
426 t_pppParam* parX = 0;
427 t_pppParam* parY = 0;
428 t_pppParam* parZ = 0;
429 _numSat = 0;
430 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
431 t_pppSatObs *obs = obsVector[ii];
432 if (obs->isValid() && !obs->outlier()) {
433 ++_numSat;
434 for (unsigned iPar = 0; iPar < numPar; iPar++) {
435 t_pppParam* par = _parlist->params()[iPar];
436 AA[_numSat - 1][iPar] = par->partial(_epoTime, obs, t_lc::c1);
437 if (par->type() == t_pppParam::crdX) {
438 parX = par;
439 }
440 else if (par->type() == t_pppParam::crdY) {
441 parY = par;
442 }
443 else if (par->type() == t_pppParam::crdZ) {
444 parZ = par;
445 }
446 }
447 }
448 }
449 if (_numSat < 4) {
450 return;
451 }
452 AA = AA.Rows(1, _numSat);
453 SymmetricMatrix NN; NN << AA.t() * AA;
454 SymmetricMatrix QQ = NN.i();
455 SymmetricMatrix QQxyz = QQ.SymSubMatrix(1,3);
456
457 ColumnVector xyz(3), neu(3);
458 SymmetricMatrix QQneu(3);
459 const t_pppStation *sta = PPP_CLIENT->staRover();
460 xyz[0] = _xFlt[parX->indexNew()];
461 xyz[1] = _xFlt[parY->indexNew()];
462 xyz[2] = _xFlt[parZ->indexNew()];
463 xyz2neu(sta->ellApr().data(), xyz.data(), neu.data());
464 covariXYZ_NEU(QQxyz, sta->ellApr().data(), QQneu);
465
466 _dop.H = sqrt(QQneu(1, 1) + QQneu(2, 2));
467 _dop.V = sqrt(QQneu(3, 3));
468 _dop.P = sqrt(QQ(1, 1) + QQ(2, 2) + QQ(3, 3));
469 _dop.T = sqrt(QQ(4, 4));
470 _dop.G = sqrt(QQ(1, 1) + QQ(2, 2) + QQ(3, 3) + QQ(4, 4));
471 }
472 catch (...) {
473 }
474}
475
476//
477////////////////////////////////////////////////////////////////////////////
478void t_pppFilter::predictCovCrdPart(const SymmetricMatrix &QFltOld, bool setNeuNoiseToZero) {
479
480 const vector<t_pppParam*>& params = _parlist->params();
481
482 if (params.size() < 3) {
483 return;
484 }
485
486 bool first = (params[0]->indexOld() < 0);
487
488 SymmetricMatrix Qneu(3); Qneu = 0.0;
489 for (unsigned ii = 0; ii < 3; ii++) {
490 const t_pppParam *par = params[ii];
491 if (first) {
492 Qneu[ii][ii] = par->sigma0() * par->sigma0();
493 }
494 else {
495 Qneu[ii][ii] = par->noise() * par->noise();
496 }
497 }
498
499 const t_pppStation *sta = PPP_CLIENT->staRover();
500 SymmetricMatrix Qxyz(3);
501 covariNEU_XYZ(Qneu, sta->ellApr().data(), Qxyz);
502
503 if (first) {
504 _QFlt.SymSubMatrix(1, 3) = Qxyz;
505 }
506 else {
507 double dt = _epoTime - _firstEpoTime;
508 if (dt < OPT->_seedingTime || setNeuNoiseToZero) {
509 _QFlt.SymSubMatrix(1, 3) = QFltOld.SymSubMatrix(1, 3);
510 }
511 else {
512 _QFlt.SymSubMatrix(1, 3) = QFltOld.SymSubMatrix(1, 3) + Qxyz;
513 }
514 }
515}
516
517//
518////////////////////////////////////////////////////////////////////////////
519void t_pppFilter::setStateVectorAndVarCovMatrix(const ColumnVector &xFltOld,
520 const SymmetricMatrix &QFltOld,
521 bool setNeuNoiseToZero) {
522
523 const vector<t_pppParam*>& params = _parlist->params();
524 unsigned nPar = params.size();
525
526 _QFlt.ReSize(nPar); _QFlt = 0.0;
527 _xFlt.ReSize(nPar); _xFlt = 0.0;
528 _x0.ReSize(nPar); _x0 = 0.0;
529
530 for (unsigned ii = 0; ii < nPar; ii++) {
531 t_pppParam *par1 = params[ii];
532 if (QFltOld.size() == 0) {
533 par1->resetIndex();
534 }
535 _x0[ii] = par1->x0();
536 int iOld = par1->indexOld();
537 if (iOld < 0) {
538 _QFlt[ii][ii] = par1->sigma0() * par1->sigma0(); // new parameter
539 } else {
540 _QFlt[ii][ii] = QFltOld[iOld][iOld] + par1->noise() * par1->noise();
541 _xFlt[ii] = xFltOld[iOld];
542 for (unsigned jj = 0; jj < ii; jj++) {
543 t_pppParam *par2 = params[jj];
544 int jOld = par2->indexOld();
545 if (jOld >= 0) {
546 _QFlt[ii][jj] = QFltOld(iOld + 1, jOld + 1);
547 }
548 }
549 }
550 }
551 predictCovCrdPart(QFltOld, setNeuNoiseToZero);
552}
Note: See TracBrowser for help on using the repository browser.