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

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