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

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