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

Last change on this file since 10015 was 10015, 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: 27.1 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
30using namespace BNC_PPP;
31using namespace std;
32
33// Constructor
34////////////////////////////////////////////////////////////////////////////
35t_pppFilter::t_pppFilter(t_pppObsPool *obsPool) {
36 _numSat = 0;
37 _obsPool = obsPool;
38 _refPrn = t_prn();
39 _datumTrafo = new t_datumTrafo();
40}
41
42// Destructor
43////////////////////////////////////////////////////////////////////////////
44t_pppFilter::~t_pppFilter() {
45 delete _datumTrafo;
46}
47
48// Process Single Epoch
49////////////////////////////////////////////////////////////////////////////
50t_irc t_pppFilter::processEpoch() {
51 _numSat = 0;
52 const double maxSolGap = 60.0;
53
54 // Vector of all Observations
55 // --------------------------
56 t_pppObsPool::t_epoch *epoch = _obsPool->lastEpoch();
57 if (!epoch) {
58 return failure;
59 }
60 vector<t_pppSatObs*> &allObs = epoch->obsVector();
61
62 // Time of the Epoch
63 // -----------------
64 _epoTime = epoch->epoTime();
65
66 if (!_firstEpoTime.valid() ||
67 !_lastEpoTimeOK.valid()||
68 (maxSolGap > 0.0 && _epoTime - _lastEpoTimeOK > maxSolGap)) {
69 _firstEpoTime = _epoTime;
70 }
71
72 string epoTimeStr = string(_epoTime);
73
74 const QMap<char, t_pppRefSat*> &refSatMap = epoch->refSatMap();
75
76 const QList<char> &usedSystems = _parlist.usedSystems();
77 //--
78 // Set Parameters
79 if (_parlist.set(_epoTime, allObs, refSatMap) != success) {
80 return failure;
81 }
82 #ifdef BNC_DEBUG_PPP
83 if (OPT->_obsModelType == OPT->DCMcodeBias ||
84 OPT->_obsModelType == OPT->DCMphaseBias) {
85 _parlist.printParams(_epoTime);
86 }
87#endif
88
89 // Status Vector, Variance-Covariance Matrix
90 // -----------------------------------------
91 ColumnVector xFltOld = _xFlt;
92 SymmetricMatrix QFltOld = _QFlt;
93 bool setNeuNoiseToZero = false;
94 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
95 char sys = usedSystems[iSys];
96 int num = 0;
97 for (unsigned jj = 0; jj < allObs.size(); jj++) {
98 if (allObs[jj]->prn().system() == sys) {
99 ++num;
100 }
101 }
102 if (num < OPT->_minObs) {
103 setNeuNoiseToZero = true;
104 }
105 }
106 setStateVectorAndVarCovMatrix(xFltOld, QFltOld, setNeuNoiseToZero);
107
108 // Pre-Process Satellite Systems separately
109 // ----------------------------------------
110 bool preProcessing = false;
111 if (OPT->_obsModelType == OPT->DCMcodeBias ||
112 OPT->_obsModelType == OPT->DCMphaseBias) {
113 preProcessing = true;
114 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
115 char sys = usedSystems[iSys];
116 _refPrn.set(sys, 0);
117 if (OPT->_refSatRequired) {
118 _refPrn = refSatMap[sys]->prn();
119 }
120 vector<t_pppSatObs*> obsVector;
121 for (unsigned jj = 0; jj < allObs.size(); jj++) {
122 if (allObs[jj]->prn().system() == sys) {
123 obsVector.push_back(allObs[jj]);
124 }
125 }
126 if (iSys == 0) {
127 _datumTrafo->setFirstSystem(sys);
128 }
129 if (processSystem(OPT->LCs(sys), obsVector, _refPrn,
130 epoch->pseudoObsIono(), preProcessing) != success) {
131 LOG << sys << ": processSystem != success (pre-processing)" << endl;
132 _xFlt = xFltOld;
133 _QFlt = QFltOld;
134 _obsPool->deleteLastEpoch();
135 restoreState(2);
136 return failure;
137 }
138 }
139 // refSat change required?
140 // -----------------------
141 if (_obsPool->refSatChangeRequired()) {
142 _xFlt = xFltOld;
143 _QFlt = QFltOld;
144 return success;
145 } else if (!_obsPool->refSatChangeRequired()) {
146 initDatumTransformation(allObs, epoch->pseudoObsIono());
147 }
148 }
149
150 // Process Satellite Systems separately
151 // ------------------------------------
152 preProcessing = false;
153 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
154 char sys = usedSystems[iSys];
155 _refPrn.set(sys, 0);
156 if (OPT->_refSatRequired) {
157 _refPrn = refSatMap[sys]->prn();
158 }
159 unsigned int num = 0;
160 vector<t_pppSatObs*> obsVector;
161 for (unsigned jj = 0; jj < allObs.size(); jj++) {
162 if (allObs[jj]->prn().system() == sys) {
163 obsVector.push_back(allObs[jj]);
164 ++num;
165 }
166 }
167 if (iSys == 0 && OPT->_obsModelType == OPT->UncombPPP) {
168 _datumTrafo->setFirstSystem(sys);
169 }
170 LOG << epoTimeStr << " SATNUM " << sys << ' ' << right << setw(2) << num
171 << endl;
172 if (processSystem(OPT->LCs(sys), obsVector, _refPrn,
173 epoch->pseudoObsIono(), preProcessing) != success) {
174 LOG << "processSystem != success (fin-processing)" << endl;
175 if (OPT->_obsModelType == OPT->DCMcodeBias ||
176 OPT->_obsModelType == OPT->DCMphaseBias) {
177 _xFlt = xFltOld;
178 _QFlt = QFltOld;
179 _obsPool->deleteLastEpoch();
180 restoreState(3);
181 }
182 return failure;
183 }
184 }
185
186 // close epoch processing
187 // ----------------------
188 cmpDOP(allObs, refSatMap);
189 _parlist.printResult(_epoTime, _QFlt, _xFlt);
190 _lastEpoTimeOK = _epoTime; // remember time of last successful epoch processing
191 return success;
192}
193
194// Process Selected LCs
195////////////////////////////////////////////////////////////////////////////
196t_irc t_pppFilter::processSystem(const vector<t_lc::type> &LCs,
197 const vector<t_pppSatObs*> &obsVector, const t_prn &refPrn,
198 bool pseudoObsIonoAvailable, bool preProcessing) {
199 LOG.setf(ios::fixed);
200 char sys = refPrn.system();
201
202 // Detect Cycle Slips
203 // ------------------
204 if (detectCycleSlips(LCs, obsVector, refPrn, preProcessing) != success) {
205 return failure;
206 }
207 if (preProcessing && _obsPool->refSatChangeRequired(sys)) {
208 return success;
209 }
210
211 ColumnVector xSav = _xFlt;
212 SymmetricMatrix QSav = _QFlt;
213 string epoTimeStr = string(_epoTime);
214 const vector<t_pppParam*> &params = _parlist.params();
215 unsigned nPar = _parlist.nPar();
216
217 unsigned usedLCs = LCs.size();
218 if (OPT->_pseudoObsIono && !pseudoObsIonoAvailable) {
219 usedLCs -= 1; // GIM not used
220 }
221 // max Obs
222 unsigned maxObs = obsVector.size() * usedLCs;
223
224 if (OPT->_pseudoObsIono && pseudoObsIonoAvailable) {
225 maxObs -= 1; // pseudo obs iono with respect to refSat
226 }
227
228 // Outlier Detection Loop
229 // ----------------------
230 for (unsigned iOutlier = 0; iOutlier < maxObs; iOutlier++) {
231
232 if (iOutlier > 0) {
233 _xFlt = xSav;
234 _QFlt = QSav;
235 }
236
237 // First-Design Matrix, Terms Observed-Computed, Weight Matrix
238 // -----------------------------------------------------------
239 Matrix AA(maxObs, nPar);
240 ColumnVector ll(maxObs);
241 DiagonalMatrix PP(maxObs);
242 PP = 0.0;
243
244 int iObs = -1;
245 vector<t_pppSatObs*> usedObs;
246 vector<t_lc::type> usedTypes;
247
248 // Real Observations
249 // =================
250 double nSat = 0;
251 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
252 t_pppSatObs *obs = obsVector[ii];
253 if (iOutlier == 0 && !preProcessing) {
254 obs->resetOutlier();
255 }
256 if (!obs->outlier()) {
257 nSat++;
258 for (unsigned jj = 0; jj < usedLCs; jj++) {
259 const t_lc::type tLC = LCs[jj];
260 if (tLC == t_lc::GIM) {
261 continue;
262 }
263 ++iObs;
264 usedObs.push_back(obs);
265 usedTypes.push_back(tLC);
266 for (unsigned iPar = 0; iPar < nPar; iPar++) {
267 const t_pppParam *par = params[iPar];
268 AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
269 }
270 ll[iObs] = obs->obsValue(tLC) - obs->cmpValue(tLC)
271 - DotProduct(_x0, AA.Row(iObs + 1));
272 PP[iObs] = 1.0 / (obs->sigma(tLC) * obs->sigma(tLC));
273 }
274 }
275 }
276
277 // Check number of observations
278 // ----------------------------
279 if (iObs == -1) {
280 LOG << " number of observations == " << iObs + 1 << "\n";
281 if (preProcessing) {
282 _obsPool->setRefSatChangeRequired(sys, true);
283 return success;
284 }
285 return failure;
286 }
287
288 if ((!iOutlier) &&
289 (OPT->_obsModelType == OPT->DCMcodeBias ||
290 OPT->_obsModelType == OPT->DCMphaseBias) && (!preProcessing)) {
291 _datumTrafo->updateIndices(sys, iObs + 1);
292 _datumTrafo->prepareAA(AA.SubMatrix(1, iObs + 1, 1, _parlist.nPar()), 1);
293 }
294
295 // Pseudo Obs Iono
296 // ================
297 if (OPT->_pseudoObsIono && pseudoObsIonoAvailable) {
298 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
299 t_pppSatObs *obs = obsVector[ii];
300 if (!obs->outlier()) {
301 for (unsigned jj = 0; jj < usedLCs; jj++) {
302 const t_lc::type tLC = LCs[jj];
303 if (tLC == t_lc::GIM && !obs->isReference()) {
304 ++iObs;
305 } else {
306 continue;
307 }
308 usedObs.push_back(obs);
309 usedTypes.push_back(tLC);
310 for (unsigned iPar = 0; iPar < nPar; iPar++) {
311 const t_pppParam *par = params[iPar];
312 AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
313 }
314 ll[iObs] = obs->obsValue(tLC) - obs->cmpValue(tLC)
315 - DotProduct(_x0, AA.Row(iObs + 1));
316 PP[iObs] = 1.0 / (obs->sigma(tLC) * obs->sigma(tLC));
317 }
318 }
319 }
320 }
321
322 // Truncate matrices
323 // -----------------
324 AA = AA.Rows(1, iObs + 1);
325 ll = ll.Rows(1, iObs + 1);
326 PP = PP.SymSubMatrix(1, iObs + 1);
327
328 // Kalman update step
329 // ------------------
330 kalman(AA, ll, PP, _QFlt, _xFlt);
331
332 // Check Residuals
333 // ---------------
334 ColumnVector vv = AA * _xFlt - ll;
335 double maxOutlier = 0.0;
336 int maxOutlierIndex = -1;
337 t_lc::type maxOutlierLC = t_lc::dummy;
338 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
339 const t_lc::type tLC = usedTypes[ii];
340 double res = fabs(vv[ii]);
341 if (res > usedObs[ii]->maxRes(tLC)) {
342 if (res > fabs(maxOutlier)) {
343 maxOutlier = vv[ii];
344 maxOutlierIndex = ii;
345 maxOutlierLC = tLC;
346 }
347 }
348 }
349
350 // Mark outlier or break outlier detection loop
351 // --------------------------------------------
352 if (maxOutlierIndex > -1) {
353 t_pppSatObs *obs = usedObs[maxOutlierIndex];
354 t_pppParam *par = 0;
355 for (unsigned iPar = 0; iPar < nPar; iPar++) {
356 t_pppParam *hlp = params[iPar];
357 if (hlp->type() == t_pppParam::amb &&
358 hlp->prn() == obs->prn() &&
359 hlp->tLC() == usedTypes[maxOutlierIndex]) {
360 par = hlp;
361 }
362 }
363 if (preProcessing) {
364 if (obs->prn() == refPrn) {
365 LOG << epoTimeStr << " Outlier ("
366 << ((preProcessing) ? "pre-processing) " : "fin-processing) ")
367 << t_lc::toString(maxOutlierLC) << ' ' << obs->prn().toString()
368 << ' ' << setw(8) << setprecision(4) << maxOutlier << endl;
369 _obsPool->setRefSatChangeRequired(sys, true);
370 break;
371 } else {
372 obs->setOutlier();
373 }
374 } else { // fin-processing
375 LOG << epoTimeStr << " Outlier " << t_lc::toString(maxOutlierLC) << ' '
376 << obs->prn().toString() << ' ' << setw(8) << setprecision(4)
377 << maxOutlier << endl;
378 if (par) {
379 if (par->ambResetCandidate() ||
380 OPT->_obsModelType == OPT->DCMcodeBias ||
381 OPT->_obsModelType == OPT->DCMphaseBias) {
382 resetAmb(par->prn(), obsVector, maxOutlierLC, &QSav, &xSav);
383 }
384 else {
385 par->setAmbResetCandidate();
386 obs->setOutlier();
387 }
388 }
389 else {
390 obs->setOutlier();
391 }
392 }
393 }
394 // Print Residuals
395 // ---------------
396 else {
397 if (!preProcessing) {
398 for (unsigned jj = 0; jj < LCs.size(); jj++) {
399 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
400 const t_lc::type tLC = usedTypes[ii];
401 t_pppSatObs *obs = usedObs[ii];
402 if (tLC == LCs[jj]) {
403 obs->setRes(tLC, vv[ii]);
404 LOG << epoTimeStr << " RES " << left << setw(3)
405 << t_lc::toString(tLC) << right << ' '
406 << obs->prn().toString();
407 LOG << setw(9) << setprecision(4) << vv[ii] << endl;
408 }
409 }
410 }
411 }
412 break;
413 }
414 }
415 return success;
416}
417
418// Cycle-Slip Detection
419////////////////////////////////////////////////////////////////////////////
420t_irc t_pppFilter::detectCycleSlips(const vector<t_lc::type> &LCs,
421 const vector<t_pppSatObs*> &obsVector, const t_prn &refPrn,
422 bool preProcessing) {
423 const double SLIP = 50.0;
424 char sys = refPrn.system();
425 string epoTimeStr = string(_epoTime);
426 const vector<t_pppParam*> &params = _parlist.params();
427 unsigned nPar = _parlist.nPar();
428
429 for (unsigned ii = 0; ii < LCs.size(); ii++) {
430 const t_lc::type &tLC = LCs[ii];
431 if (t_lc::includesPhase(tLC)) {
432 for (unsigned iObs = 0; iObs < obsVector.size(); iObs++) {
433 const t_pppSatObs *obs = obsVector[iObs];
434
435 // Check set Slips and Jump Counters
436 // ---------------------------------
437 bool slip = false;
438
439 // in pre-processing only the reference satellite has to be checked
440 if (preProcessing && obs->prn() != refPrn) {
441 continue;
442 }
443
444 if (obs->slip()) {
445 LOG << epoTimeStr << "cycle slip set (obs) " << obs->prn().toString()
446 << endl;
447 slip = true;
448 }
449
450 if (_slips[obs->prn()]._obsSlipCounter != -1
451 && _slips[obs->prn()]._obsSlipCounter != obs->slipCounter()) {
452 LOG << epoTimeStr << "cycle slip set (obsSlipCounter) "
453 << obs->prn().toString() << endl;
454 slip = true;
455 }
456 if (!preProcessing) {
457 _slips[obs->prn()]._obsSlipCounter = obs->slipCounter();
458 }
459 if (_slips[obs->prn()]._biasJumpCounter != -1
460 && _slips[obs->prn()]._biasJumpCounter != obs->biasJumpCounter()) {
461 LOG << epoTimeStr << "cycle slip set (biasJumpCounter) "
462 << obs->prn().toString() << endl;
463 slip = true;
464 }
465 if (!preProcessing) {
466 _slips[obs->prn()]._biasJumpCounter = obs->biasJumpCounter();
467 }
468 // Slip Set
469 // --------
470 if (slip) {
471 if (preProcessing) {
472 _obsPool->setRefSatChangeRequired(sys, true);
473 } else {
474 resetAmb(obs->prn(), obsVector, tLC);
475 }
476 }
477 // Check Pre-Fit Residuals
478 // -----------------------
479 else {
480 ColumnVector AA(nPar);
481 for (unsigned iPar = 0; iPar < nPar; iPar++) {
482 const t_pppParam *par = params[iPar];
483 AA[iPar] = par->partial(_epoTime, obs, tLC, refPrn);
484 }
485 double ll = obs->obsValue(tLC) - obs->cmpValue(tLC)
486 - DotProduct(_x0, AA);
487 double vv = DotProduct(AA, _xFlt) - ll;
488 if (fabs(vv) > SLIP) {
489 LOG << epoTimeStr << " cycle slip detected " << t_lc::toString(tLC)
490 << ' ' << obs->prn().toString() << ' ' << setw(8)
491 << setprecision(4) << vv << endl;
492 if (preProcessing) {
493 _obsPool->setRefSatChangeRequired(sys, true);
494 } else {
495 resetAmb(obs->prn(), obsVector, tLC);
496 }
497 }
498 }
499 }
500 }
501 }
502 return success;
503}
504
505// Reset Ambiguity Parameter (cycle slip)
506////////////////////////////////////////////////////////////////////////////
507t_irc t_pppFilter::resetAmb(t_prn prn, const vector<t_pppSatObs*> &obsVector,
508 t_lc::type lc, SymmetricMatrix *QSav, ColumnVector *xSav) {
509
510 t_irc irc = failure;
511 vector<t_pppParam*> &params = _parlist.params();
512 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
513 t_pppParam *par = params[iPar];
514 if (par->type() == t_pppParam::amb && par->prn() == prn) {
515 int ind = par->indexNew();
516 t_lc::type tLC = par->tLC();
517 if (tLC != lc) {
518 continue;
519 }
520 LOG << string(_epoTime) << " RESET " << par->toString() << endl;
521 delete par;
522 par = new t_pppParam(t_pppParam::amb, prn, tLC, &obsVector);
523 par->setIndex(ind);
524 params[iPar] = par;
525 for (unsigned ii = 1; ii <= params.size(); ii++) {
526 _QFlt(ii, ind + 1) = 0.0;
527 if (QSav) {
528 (*QSav)(ii, ind + 1) = 0.0;
529 }
530 }
531 _QFlt(ind + 1, ind + 1) = par->sigma0() * par->sigma0();
532 if (QSav) {
533 (*QSav)(ind + 1, ind + 1) = _QFlt(ind + 1, ind + 1);
534 }
535 _xFlt[ind] = 0.0;
536 if (xSav) {
537 (*xSav)[ind] = _xFlt[ind];
538 }
539 _x0[ind] = par->x0();
540 irc = success;
541 }
542 }
543
544 return irc;
545}
546
547// Add infinite noise to iono
548////////////////////////////////////////////////////////////////////////////
549t_irc t_pppFilter::addNoiseToPar(t_pppParam::e_type parType, char sys) {
550 t_irc irc = failure;
551 vector<t_pppParam*> &params = _parlist.params();
552 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
553 t_pppParam *par = params[iPar];
554 if (par->type() == parType && par->prn().system() == sys) {
555 int ind = par->indexNew();
556 LOG << string(_epoTime) << " ADD NOISE TO " << par->toString() << endl;
557 par->setIndex(ind);
558 _QFlt(ind + 1, ind + 1) = par->sigma0() * par->sigma0();
559 irc = success;
560 }
561 }
562
563 return irc;
564}
565
566// Compute various DOP Values
567////////////////////////////////////////////////////////////////////////////
568void t_pppFilter::cmpDOP(const vector<t_pppSatObs*> &obsVector,
569 const QMap<char, t_pppRefSat*> &refSatMap) {
570
571 _dop.reset();
572
573 try {
574 const unsigned numPar = 4;
575 Matrix AA(obsVector.size(), numPar);
576 _numSat = 0;
577 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
578 t_pppSatObs *obs = obsVector[ii];
579 char sys = obs->prn().system();
580 t_prn refPrn = t_prn();
581 if (OPT->_refSatRequired) {
582 refPrn = refSatMap[sys]->prn();
583 }
584 if (obs->isValid() && !obs->outlier()) {
585 ++_numSat;
586 for (unsigned iPar = 0; iPar < numPar; iPar++) {
587 const t_pppParam *par = _parlist.params()[iPar];
588 AA[_numSat - 1][iPar] = par->partial(_epoTime, obs, t_lc::c1, refPrn);
589 }
590 }
591 }
592 if (_numSat < 4) {
593 return;
594 }
595 AA = AA.Rows(1, _numSat);
596 SymmetricMatrix NN;
597 NN << AA.t() * AA;
598 SymmetricMatrix QQ = NN.i();
599
600 _dop.H = sqrt(QQ(1, 1) + QQ(2, 2));
601 _dop.V = sqrt(QQ(3, 3));
602 _dop.P = sqrt(QQ(1, 1) + QQ(2, 2) + QQ(3, 3));
603 _dop.T = sqrt(QQ(4, 4));
604 _dop.G = sqrt(QQ(1, 1) + QQ(2, 2) + QQ(3, 3) + QQ(4, 4));
605 } catch (...) {
606 }
607}
608
609//
610////////////////////////////////////////////////////////////////////////////
611void t_pppFilter::predictCovCrdPart(const SymmetricMatrix &QFltOld, bool setNeuNoiseToZero) {
612
613 const vector<t_pppParam*> &params = _parlist.params();
614 if (params.size() < 3) {
615 return;
616 }
617
618 bool first = (params[0]->indexOld() < 0);
619
620 SymmetricMatrix Qneu(3);
621 Qneu = 0.0;
622 for (unsigned ii = 0; ii < 3; ii++) {
623 const t_pppParam *par = params[ii];
624 if (first) {
625 Qneu[ii][ii] = par->sigma0() * par->sigma0();
626 } else {
627 Qneu[ii][ii] = par->noise() * par->noise();
628 }
629 }
630
631 const t_pppStation *sta = PPP_CLIENT->staRover();
632 SymmetricMatrix Qxyz(3);
633 covariNEU_XYZ(Qneu, sta->ellApr().data(), Qxyz);
634
635 if (first) {
636 _QFlt.SymSubMatrix(1, 3) = Qxyz;
637 } else {
638 double dt = _epoTime - _firstEpoTime;
639 if (dt < OPT->_seedingTime || setNeuNoiseToZero) {
640 _QFlt.SymSubMatrix(1, 3) = QFltOld.SymSubMatrix(1, 3);
641 } else {
642 _QFlt.SymSubMatrix(1, 3) = QFltOld.SymSubMatrix(1, 3) + Qxyz;
643 }
644 }
645}
646
647//
648////////////////////////////////////////////////////////////////////////////
649void t_pppFilter::setStateVectorAndVarCovMatrix(const ColumnVector &xFltOld,
650 const SymmetricMatrix &QFltOld, bool setNeuNoiseToZero) {
651
652 const vector<t_pppParam*> &params = _parlist.params();
653 unsigned nPar = params.size();
654
655 _QFlt.ReSize(nPar);
656 _QFlt = 0.0;
657 _xFlt.ReSize(nPar);
658 _xFlt = 0.0;
659 _x0.ReSize(nPar);
660 _x0 = 0.0;
661
662 for (unsigned ii = 0; ii < nPar; ii++) {
663 t_pppParam *par1 = params[ii];
664 if (QFltOld.size() == 0) {
665 par1->resetIndex();
666 }
667 _x0[ii] = par1->x0();
668 int iOld = par1->indexOld();
669 if (iOld < 0) {
670 _QFlt[ii][ii] = par1->sigma0() * par1->sigma0(); // new parameter
671 } else {
672 _QFlt[ii][ii] = QFltOld[iOld][iOld] + par1->noise() * par1->noise();
673 _xFlt[ii] = xFltOld[iOld];
674 for (unsigned jj = 0; jj < ii; jj++) {
675 t_pppParam *par2 = params[jj];
676 int jOld = par2->indexOld();
677 if (jOld >= 0) {
678 _QFlt[ii][jj] = QFltOld(iOld + 1, jOld + 1);
679 }
680 }
681 }
682 }
683 predictCovCrdPart(QFltOld, setNeuNoiseToZero);
684}
685
686// Compute datum transformation
687////////////////////////////////////////////////////////////////////////////
688t_irc t_pppFilter::datumTransformation(
689 const QMap<char, t_pppRefSat*> &refSatMap) {
690
691 // get last epoch
692 t_pppObsPool::t_epoch *epoch = _obsPool->lastEpoch();
693 if (!epoch) {
694 LOG << "t_pppFilter::datumTransformation: !lastEpoch" << endl;
695 return failure;
696 }
697 _epoTime = epoch->epoTime();
698 LOG.setf(ios::fixed);
699 LOG << string(_epoTime) << "\nDATUM TRANSFORMATION " << endl;
700
701 vector<t_pppSatObs*> &allObs = epoch->obsVector();
702
703 const QMap<char, t_pppRefSat*> &refSatMapLastEpoch = epoch->refSatMap();
704
705 bool pseudoObsIono = epoch->pseudoObsIono();
706
707 // reset old and set new refSats in last epoch (ambiguities/GIM)
708 // =============================================================
709 if (resetRefSatellitesLastEpoch(allObs, refSatMap, refSatMapLastEpoch) != true) {
710 LOG << "datumTransformation: resetRefSatellitesLastEpoch != success" << endl;
711 return failure;
712 }
713
714 if (OPT->_obsModelType == OPT->UncombPPP) {
715 _obsPool->putEpoch(_epoTime, allObs, pseudoObsIono, refSatMap);
716 return success;
717 }
718
719 // set AA2
720 // =======
721 if (_parlist.set(_epoTime, allObs, refSatMap) != success) {
722 return failure;
723 }
724#ifdef BNC_DEBUG_PPP
725 _parlist.printParams(_epoTime);
726#endif
727
728 const QList<char> &usedSystems = _parlist.usedSystems();
729 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
730 char sys = usedSystems[iSys];
731 t_prn refPrn = refSatMap[sys]->prn();
732 vector<t_pppSatObs*> obsVector;
733 for (unsigned jj = 0; jj < allObs.size(); jj++) {
734 if (allObs[jj]->prn().system() == sys) {
735 allObs[jj]->resetOutlier();
736 obsVector.push_back(allObs[jj]);
737 }
738 }
739 if (iSys == 0) {
740 _datumTrafo->setFirstSystem(sys);
741 }
742 vector<t_lc::type> LCs = OPT->LCs(sys);
743 unsigned usedLCs = LCs.size();
744 if (OPT->_pseudoObsIono && !pseudoObsIono) {
745 usedLCs -= 1; // GIM not used
746 }
747 // max Obs
748 unsigned maxObs = obsVector.size() * usedLCs;
749
750 if (OPT->_pseudoObsIono && pseudoObsIono) {
751 maxObs -= 1; // pseudo obs iono with respect to refSat
752 }
753
754 const vector<t_pppParam*> &params = _parlist.params();
755 unsigned nPar = _parlist.nPar();
756
757 Matrix AA(maxObs, nPar); AA = 0.0; cout << "AA Ncols/Nrows: " << AA.Ncols() << "/" << AA.Nrows() << " nPar: " << nPar << endl;
758
759 // Real Observations
760 // -----------------
761 int iObs = -1;
762 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
763 t_pppSatObs *obs = obsVector[ii];
764 for (unsigned jj = 0; jj < usedLCs; jj++) {
765 const t_lc::type tLC = LCs[jj];
766 if (tLC == t_lc::GIM) {
767 continue;
768 }
769 ++iObs;
770 for (unsigned iPar = 0; iPar < nPar; iPar++) {
771 const t_pppParam *par = params[iPar];
772 AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
773 }
774 }
775 }
776
777 if (!(iObs+1)) {
778 continue;
779 }
780 _datumTrafo->updateIndices(sys, iObs + 1);
781
782 if (_datumTrafo->prepareAA(AA.SubMatrix(1, iObs + 1, 1, nPar), 2) != success) {
783 return failure;
784 }
785 }
786 _datumTrafo->updateNumObs();
787
788 // Datum Transformation
789 // ====================
790 if (_datumTrafo->computeTrafoMatrix() != success) {
791 return failure;
792 }
793
794 ColumnVector xFltOld = _xFlt;
795 SymmetricMatrix QFltOld = _QFlt;
796
797 _QFlt << _datumTrafo->D21() * QFltOld * _datumTrafo->D21().t();
798 _xFlt = _datumTrafo->D21() * xFltOld;
799
800#ifdef BNC_DEBUG_PPP
801 //LOG << "xFltOld:\n" << xFltOld << endl;
802 //LOG << "xFlt :\n" << _xFlt << endl;
803#endif
804
805 // Reset Ambiguities after Datum Transformation
806 // ============================================
807 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
808 char sys = usedSystems[iSys];
809 vector<t_lc::type> LCs = OPT->LCs(sys);
810 unsigned usedLCs = LCs.size();
811 if (OPT->_pseudoObsIono && !pseudoObsIono) {
812 usedLCs -= 1; // GIM not used
813 }
814 t_prn refPrnOld = refSatMapLastEpoch[sys]->prn();
815 t_prn refPrnNew = refSatMap[sys]->prn();
816 if (refPrnNew != refPrnOld) {
817 for (unsigned jj = 0; jj < usedLCs; jj++) {
818 const t_lc::type tLC = LCs[jj];
819 if (tLC == t_lc::GIM) {
820 continue;
821 }
822 resetAmb(refPrnOld, allObs, tLC);
823 }
824 }
825 }
826
827 // switch AA2 to AA1
828 // =================
829 _datumTrafo->switchAA();
830
831 _obsPool->putEpoch(_epoTime, allObs, pseudoObsIono, refSatMap);
832
833 return success;
834}
835
836// Init datum transformation
837////////////////////////////////////////////////////////////////////////////
838void t_pppFilter::initDatumTransformation(
839 const std::vector<t_pppSatObs*> &allObs, bool pseudoObsIono) {
840 unsigned trafoObs = 0;
841 const QList<char> &usedSystems = _parlist.usedSystems();
842 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
843 char sys = usedSystems[iSys];
844 int satNum = 0;
845 for (unsigned jj = 0; jj < allObs.size(); jj++) {
846 if (allObs[jj]->prn().system() == sys) {
847 satNum++;
848 }
849 }
850 // all LCs
851 unsigned realUsedLCs = OPT->LCs(sys).size();
852 // exclude pseudo obs GIM
853 if (OPT->_pseudoObsIono && !pseudoObsIono) {
854 realUsedLCs -= 1;
855 }
856
857 trafoObs += satNum * realUsedLCs;
858
859 if (OPT->_pseudoObsIono && pseudoObsIono) {
860 trafoObs -= 1; // pseudo obs iono with respect to refSat
861 }
862 }
863 _datumTrafo->setNumObs(trafoObs);
864 _datumTrafo->setNumPar(_parlist.nPar());
865 _datumTrafo->initAA();
866}
867
868//
869//////////////////////////////////////////////////////////////////////////////
870bool t_pppFilter::resetRefSatellitesLastEpoch(
871 std::vector<t_pppSatObs*> &obsVector,
872 const QMap<char, t_pppRefSat*> &refSatMap,
873 const QMap<char, t_pppRefSat*> &refSatMapLastEpoch) {
874 bool resetRefSat;
875 // reference satellite definition per system
876 const QList<char> &usedSystems = _parlist.usedSystems();
877 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
878 resetRefSat = false;
879 char sys = usedSystems[iSys];
880 t_prn newPrn = refSatMap[sys]->prn();
881 t_prn oldPrn = refSatMapLastEpoch[sys]->prn();
882#ifdef BNC_DEBUG_PPP
883 if (oldPrn != newPrn) {
884 LOG << "oldRef: " << oldPrn.toString() << " => newRef " << newPrn.toString() << endl;
885 }
886#endif
887 vector<t_pppSatObs*>::iterator it = obsVector.begin();
888 while (it != obsVector.end()) {
889 t_pppSatObs *satObs = *it;
890 if (satObs->prn() == newPrn) {
891 resetRefSat = true;
892 satObs->setAsReference();
893 } else if (satObs->prn() == oldPrn) {
894 satObs->resetReference();
895 }
896 it++;
897 }
898 if (!resetRefSat) {
899 _obsPool->setRefSatChangeRequired(sys, true);
900 return resetRefSat;
901 }
902 }
903 return resetRefSat;
904}
Note: See TracBrowser for help on using the repository browser.