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

Last change on this file since 10018 was 10018, 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.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() ||
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 restoreState(1);
135 return failure;
136 }
137 }
138 // refSat change required?
139 // -----------------------
140 if (_obsPool->refSatChangeRequired()) {
141 _xFlt = xFltOld;
142 _QFlt = QFltOld;
143 return success;
144 } else if (!_obsPool->refSatChangeRequired()) {
145 initDatumTransformation(allObs, epoch->pseudoObsIono());
146 }
147 }
148
149 // Process Satellite Systems separately
150 // ------------------------------------
151 preProcessing = false;
152 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
153 char sys = usedSystems[iSys];
154 _refPrn.set(sys, 0);
155 if (OPT->_refSatRequired) {
156 _refPrn = refSatMap[sys]->prn();
157 }
158 unsigned int num = 0;
159 vector<t_pppSatObs*> obsVector;
160 for (unsigned jj = 0; jj < allObs.size(); jj++) {
161 if (allObs[jj]->prn().system() == sys) {
162 obsVector.push_back(allObs[jj]);
163 ++num;
164 }
165 }
166 if (iSys == 0 && OPT->_obsModelType == OPT->UncombPPP) {
167 _datumTrafo->setFirstSystem(sys);
168 }
169 LOG << epoTimeStr << " SATNUM " << sys << ' ' << right << setw(2) << num
170 << endl;
171 if (processSystem(OPT->LCs(sys), obsVector, _refPrn,
172 epoch->pseudoObsIono(), preProcessing) != success) {
173 LOG << "processSystem != success (fin-processing)" << endl;
174 if (OPT->_obsModelType == OPT->DCMcodeBias ||
175 OPT->_obsModelType == OPT->DCMphaseBias) {
176 _xFlt = xFltOld;
177 _QFlt = QFltOld;
178 restoreState(2);
179 }
180 return failure;
181 }
182 }
183
184 // close epoch processing
185 // ----------------------
186 cmpDOP(allObs, refSatMap);
187 _parlist.printResult(_epoTime, _QFlt, _xFlt);
188 _lastEpoTimeOK = _epoTime; // remember time of last successful epoch processing
189 return success;
190}
191
192// Process Selected LCs
193////////////////////////////////////////////////////////////////////////////
194t_irc t_pppFilter::processSystem(const vector<t_lc::type> &LCs,
195 const vector<t_pppSatObs*> &obsVector, const t_prn &refPrn,
196 bool pseudoObsIonoAvailable, bool preProcessing) {
197 LOG.setf(ios::fixed);
198 char sys = refPrn.system();
199
200 // Detect Cycle Slips
201 // ------------------
202 if (detectCycleSlips(LCs, obsVector, refPrn, preProcessing) != success) {
203 return failure;
204 }
205 if (preProcessing && _obsPool->refSatChangeRequired(sys)) {
206 return success;
207 }
208
209 ColumnVector xSav = _xFlt;
210 SymmetricMatrix QSav = _QFlt;
211 string epoTimeStr = string(_epoTime);
212 const vector<t_pppParam*> &params = _parlist.params();
213 unsigned nPar = _parlist.nPar();
214
215 unsigned usedLCs = LCs.size();
216 if (OPT->_pseudoObsIono && !pseudoObsIonoAvailable) {
217 usedLCs -= 1; // GIM not used
218 }
219 // max Obs
220 unsigned maxObs = obsVector.size() * usedLCs;
221
222 if (OPT->_pseudoObsIono && pseudoObsIonoAvailable) {
223 maxObs -= 1; // pseudo obs iono with respect to refSat
224 }
225
226 // Outlier Detection Loop
227 // ----------------------
228 for (unsigned iOutlier = 0; iOutlier < maxObs; iOutlier++) {
229
230 if (iOutlier > 0) {
231 _xFlt = xSav;
232 _QFlt = QSav;
233 }
234
235 // First-Design Matrix, Terms Observed-Computed, Weight Matrix
236 // -----------------------------------------------------------
237 Matrix AA(maxObs, nPar);
238 ColumnVector ll(maxObs);
239 DiagonalMatrix PP(maxObs);
240 PP = 0.0;
241
242 int iObs = -1;
243 vector<t_pppSatObs*> usedObs;
244 vector<t_lc::type> usedTypes;
245
246 // Real Observations
247 // =================
248 double nSat = 0;
249 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
250 t_pppSatObs *obs = obsVector[ii];
251 if (iOutlier == 0 && !preProcessing) {
252 obs->resetOutlier();
253 }
254 if (!obs->outlier()) {
255 nSat++;
256 for (unsigned jj = 0; jj < usedLCs; jj++) {
257 const t_lc::type tLC = LCs[jj];
258 if (tLC == t_lc::GIM) {
259 continue;
260 }
261 ++iObs;
262 usedObs.push_back(obs);
263 usedTypes.push_back(tLC);
264 for (unsigned iPar = 0; iPar < nPar; iPar++) {
265 const t_pppParam *par = params[iPar];
266 AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
267 }
268 ll[iObs] = obs->obsValue(tLC) - obs->cmpValue(tLC)
269 - DotProduct(_x0, AA.Row(iObs + 1));
270 PP[iObs] = 1.0 / (obs->sigma(tLC) * obs->sigma(tLC));
271 }
272 }
273 }
274
275 // Check number of observations
276 // ----------------------------
277 if ((iObs +1) < OPT->_minObs) {
278 LOG << "t_pppFilter::processSystem not enough observations: " << iObs + 1 << "\n";
279 if (preProcessing) {
280 _obsPool->setRefSatChangeRequired(sys, true);
281 return success;
282 }
283 return failure;
284 }
285
286 if ((!iOutlier) &&
287 (OPT->_obsModelType == OPT->DCMcodeBias ||
288 OPT->_obsModelType == OPT->DCMphaseBias) && (!preProcessing)) {
289 _datumTrafo->updateIndices(sys, iObs + 1);
290 _datumTrafo->prepareAA(AA.SubMatrix(1, iObs + 1, 1, _parlist.nPar()), 1);
291 }
292
293 // Pseudo Obs Iono
294 // ================
295 if (OPT->_pseudoObsIono && pseudoObsIonoAvailable) {
296 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
297 t_pppSatObs *obs = obsVector[ii];
298 if (!obs->outlier()) {
299 for (unsigned jj = 0; jj < usedLCs; jj++) {
300 const t_lc::type tLC = LCs[jj];
301 if (tLC == t_lc::GIM && !obs->isReference()) {
302 ++iObs;
303 } else {
304 continue;
305 }
306 usedObs.push_back(obs);
307 usedTypes.push_back(tLC);
308 for (unsigned iPar = 0; iPar < nPar; iPar++) {
309 const t_pppParam *par = params[iPar];
310 AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
311 }
312 ll[iObs] = obs->obsValue(tLC) - obs->cmpValue(tLC)
313 - DotProduct(_x0, AA.Row(iObs + 1));
314 PP[iObs] = 1.0 / (obs->sigma(tLC) * obs->sigma(tLC));
315 }
316 }
317 }
318 }
319
320 // Truncate matrices
321 // -----------------
322 AA = AA.Rows(1, iObs + 1);
323 ll = ll.Rows(1, iObs + 1);
324 PP = PP.SymSubMatrix(1, iObs + 1);
325
326 // Kalman update step
327 // ------------------
328 kalman(AA, ll, PP, _QFlt, _xFlt);
329
330 // Check Residuals
331 // ---------------
332 ColumnVector vv = AA * _xFlt - ll;
333 double maxOutlier = 0.0;
334 int maxOutlierIndex = -1;
335 t_lc::type maxOutlierLC = t_lc::dummy;
336 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
337 const t_lc::type tLC = usedTypes[ii];
338 double res = fabs(vv[ii]);
339 if (res > usedObs[ii]->maxRes(tLC)) {
340 if (res > fabs(maxOutlier)) {
341 maxOutlier = vv[ii];
342 maxOutlierIndex = ii;
343 maxOutlierLC = tLC;
344 }
345 }
346 }
347
348 // Mark outlier or break outlier detection loop
349 // --------------------------------------------
350 if (maxOutlierIndex > -1) {
351 t_pppSatObs *obs = usedObs[maxOutlierIndex];
352 t_pppParam *par = 0;
353 for (unsigned iPar = 0; iPar < nPar; iPar++) {
354 t_pppParam *hlp = params[iPar];
355 if (hlp->type() == t_pppParam::amb &&
356 hlp->prn() == obs->prn() &&
357 hlp->tLC() == usedTypes[maxOutlierIndex]) {
358 par = hlp;
359 }
360 }
361 if (preProcessing) {
362 if (obs->prn() == refPrn) {
363 LOG << epoTimeStr << " Outlier ("
364 << ((preProcessing) ? "pre-processing) " : "fin-processing) ")
365 << t_lc::toString(maxOutlierLC) << ' ' << obs->prn().toString()
366 << ' ' << setw(8) << setprecision(4) << maxOutlier << endl;
367 _obsPool->setRefSatChangeRequired(sys, true);
368 break;
369 }
370 else {
371 obs->setOutlier();
372 }
373 } else { // fin-processing
374 if (obs->prn() != refPrn) {
375 LOG << epoTimeStr << " Outlier " << t_lc::toString(maxOutlierLC) << ' '
376 << obs->prn().toString() << ' ' << setw(8) << setprecision(4)
377 << maxOutlier << endl;
378 if (par) {
379 resetAmb(par->prn(), obsVector, maxOutlierLC, &QSav, &xSav);
380 }
381 else {
382 obs->setOutlier();
383 }
384 }
385 }
386 }
387 // Print Residuals
388 // ---------------
389 else {
390 if (!preProcessing) {
391 for (unsigned jj = 0; jj < LCs.size(); jj++) {
392 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
393 const t_lc::type tLC = usedTypes[ii];
394 t_pppSatObs *obs = usedObs[ii];
395 if (tLC == LCs[jj]) {
396 obs->setRes(tLC, vv[ii]);
397 LOG << epoTimeStr << " RES " << left << setw(3)
398 << t_lc::toString(tLC) << right << ' '
399 << obs->prn().toString();
400 LOG << setw(9) << setprecision(4) << vv[ii] << endl;
401 }
402 }
403 }
404 }
405 break;
406 }
407 }
408 return success;
409}
410
411// Cycle-Slip Detection
412////////////////////////////////////////////////////////////////////////////
413t_irc t_pppFilter::detectCycleSlips(const vector<t_lc::type> &LCs,
414 const vector<t_pppSatObs*> &obsVector, const t_prn &refPrn,
415 bool preProcessing) {
416 const double SLIP = 50.0;
417 char sys = refPrn.system();
418 string epoTimeStr = string(_epoTime);
419 const vector<t_pppParam*> &params = _parlist.params();
420 unsigned nPar = _parlist.nPar();
421
422 for (unsigned ii = 0; ii < LCs.size(); ii++) {
423 const t_lc::type &tLC = LCs[ii];
424 if (t_lc::includesPhase(tLC)) {
425 for (unsigned iObs = 0; iObs < obsVector.size(); iObs++) {
426 const t_pppSatObs *obs = obsVector[iObs];
427
428 // Check set Slips and Jump Counters
429 // ---------------------------------
430 bool slip = false;
431
432 // in pre-processing only the reference satellite has to be checked
433 if (preProcessing && obs->prn() != refPrn) {
434 continue;
435 }
436
437 if (obs->slip()) {
438 LOG << epoTimeStr << "cycle slip set (obs) " << obs->prn().toString()
439 << endl;
440 slip = true;
441 }
442
443 if (_slips[obs->prn()]._obsSlipCounter != -1
444 && _slips[obs->prn()]._obsSlipCounter != obs->slipCounter()) {
445 LOG << epoTimeStr << "cycle slip set (obsSlipCounter) "
446 << obs->prn().toString() << endl;
447 slip = true;
448 }
449 if (!preProcessing) {
450 _slips[obs->prn()]._obsSlipCounter = obs->slipCounter();
451 }
452 if (_slips[obs->prn()]._biasJumpCounter != -1
453 && _slips[obs->prn()]._biasJumpCounter != obs->biasJumpCounter()) {
454 LOG << epoTimeStr << "cycle slip set (biasJumpCounter) "
455 << obs->prn().toString() << endl;
456 slip = true;
457 }
458 if (!preProcessing) {
459 _slips[obs->prn()]._biasJumpCounter = obs->biasJumpCounter();
460 }
461 // Slip Set
462 // --------
463 if (slip) {
464 if (preProcessing) {
465 _obsPool->setRefSatChangeRequired(sys, true);
466 } else {
467 resetAmb(obs->prn(), obsVector, tLC);
468 }
469 }
470 // Check Pre-Fit Residuals
471 // -----------------------
472 else {
473 ColumnVector AA(nPar);
474 for (unsigned iPar = 0; iPar < nPar; iPar++) {
475 const t_pppParam *par = params[iPar];
476 AA[iPar] = par->partial(_epoTime, obs, tLC, refPrn);
477 }
478 double ll = obs->obsValue(tLC) - obs->cmpValue(tLC)
479 - DotProduct(_x0, AA);
480 double vv = DotProduct(AA, _xFlt) - ll;
481 if (fabs(vv) > SLIP) {
482 LOG << epoTimeStr << " cycle slip detected " << t_lc::toString(tLC)
483 << ' ' << obs->prn().toString() << ' ' << setw(8)
484 << setprecision(4) << vv << endl;
485 if (preProcessing) {
486 _obsPool->setRefSatChangeRequired(sys, true);
487 } else {
488 resetAmb(obs->prn(), obsVector, tLC);
489 }
490 }
491 }
492 }
493 }
494 }
495 return success;
496}
497
498// Reset Ambiguity Parameter (cycle slip)
499////////////////////////////////////////////////////////////////////////////
500t_irc t_pppFilter::resetAmb(t_prn prn, const vector<t_pppSatObs*> &obsVector,
501 t_lc::type lc, SymmetricMatrix *QSav, ColumnVector *xSav) {
502
503 t_irc irc = failure;
504 vector<t_pppParam*> &params = _parlist.params();
505 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
506 t_pppParam *par = params[iPar];
507 if (par->type() == t_pppParam::amb && par->prn() == prn) {
508 int ind = par->indexNew();
509 t_lc::type tLC = par->tLC();
510 if (tLC != lc) {
511 continue;
512 }
513 LOG << string(_epoTime) << " RESET " << par->toString() << endl;
514 delete par;
515 par = new t_pppParam(t_pppParam::amb, prn, tLC, &obsVector);
516 par->setIndex(ind);
517 params[iPar] = par;
518 for (unsigned ii = 1; ii <= params.size(); ii++) {
519 _QFlt(ii, ind + 1) = 0.0;
520 if (QSav) {
521 (*QSav)(ii, ind + 1) = 0.0;
522 }
523 }
524 _QFlt(ind + 1, ind + 1) = par->sigma0() * par->sigma0();
525 if (QSav) {
526 (*QSav)(ind + 1, ind + 1) = _QFlt(ind + 1, ind + 1);
527 }
528 _xFlt[ind] = 0.0;
529 if (xSav) {
530 (*xSav)[ind] = _xFlt[ind];
531 }
532 _x0[ind] = par->x0();
533 irc = success;
534 }
535 }
536
537 return irc;
538}
539
540// Add infinite noise to iono
541////////////////////////////////////////////////////////////////////////////
542t_irc t_pppFilter::addNoiseToPar(t_pppParam::e_type parType, char sys) {
543 t_irc irc = failure;
544 vector<t_pppParam*> &params = _parlist.params();
545 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
546 t_pppParam *par = params[iPar];
547 if (par->type() == parType && par->prn().system() == sys) {
548 int ind = par->indexNew();
549 LOG << string(_epoTime) << " ADD NOISE TO " << par->toString() << endl;
550 par->setIndex(ind);
551 _QFlt(ind + 1, ind + 1) = par->sigma0() * par->sigma0();
552 irc = success;
553 }
554 }
555
556 return irc;
557}
558
559// Compute various DOP Values
560////////////////////////////////////////////////////////////////////////////
561void t_pppFilter::cmpDOP(const vector<t_pppSatObs*> &obsVector,
562 const QMap<char, t_pppRefSat*> &refSatMap) {
563
564 _dop.reset();
565
566 try {
567 const unsigned numPar = 4;
568 Matrix AA(obsVector.size(), numPar);
569 _numSat = 0;
570 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
571 t_pppSatObs *obs = obsVector[ii];
572 char sys = obs->prn().system();
573 t_prn refPrn = t_prn();
574 if (OPT->_refSatRequired) {
575 refPrn = refSatMap[sys]->prn();
576 }
577 if (obs->isValid() && !obs->outlier()) {
578 ++_numSat;
579 for (unsigned iPar = 0; iPar < numPar; iPar++) {
580 const t_pppParam *par = _parlist.params()[iPar];
581 AA[_numSat - 1][iPar] = par->partial(_epoTime, obs, t_lc::c1, refPrn);
582 }
583 }
584 }
585 if (_numSat < 4) {
586 return;
587 }
588 AA = AA.Rows(1, _numSat);
589 SymmetricMatrix NN;
590 NN << AA.t() * AA;
591 SymmetricMatrix QQ = NN.i();
592
593 _dop.H = sqrt(QQ(1, 1) + QQ(2, 2));
594 _dop.V = sqrt(QQ(3, 3));
595 _dop.P = sqrt(QQ(1, 1) + QQ(2, 2) + QQ(3, 3));
596 _dop.T = sqrt(QQ(4, 4));
597 _dop.G = sqrt(QQ(1, 1) + QQ(2, 2) + QQ(3, 3) + QQ(4, 4));
598 } catch (...) {
599 }
600}
601
602//
603////////////////////////////////////////////////////////////////////////////
604void t_pppFilter::predictCovCrdPart(const SymmetricMatrix &QFltOld, bool setNeuNoiseToZero) {
605
606 const vector<t_pppParam*> &params = _parlist.params();
607 if (params.size() < 3) {
608 return;
609 }
610
611 bool first = (params[0]->indexOld() < 0);
612
613 SymmetricMatrix Qneu(3);
614 Qneu = 0.0;
615 for (unsigned ii = 0; ii < 3; ii++) {
616 const t_pppParam *par = params[ii];
617 if (first) {
618 Qneu[ii][ii] = par->sigma0() * par->sigma0();
619 } else {
620 Qneu[ii][ii] = par->noise() * par->noise();
621 }
622 }
623
624 const t_pppStation *sta = PPP_CLIENT->staRover();
625 SymmetricMatrix Qxyz(3);
626 covariNEU_XYZ(Qneu, sta->ellApr().data(), Qxyz);
627
628 if (first) {
629 _QFlt.SymSubMatrix(1, 3) = Qxyz;
630 } else {
631 double dt = _epoTime - _firstEpoTime;
632 if (dt < OPT->_seedingTime || setNeuNoiseToZero) {
633 _QFlt.SymSubMatrix(1, 3) = QFltOld.SymSubMatrix(1, 3);
634 } else {
635 _QFlt.SymSubMatrix(1, 3) = QFltOld.SymSubMatrix(1, 3) + Qxyz;
636 }
637 }
638}
639
640//
641////////////////////////////////////////////////////////////////////////////
642void t_pppFilter::setStateVectorAndVarCovMatrix(const ColumnVector &xFltOld,
643 const SymmetricMatrix &QFltOld, bool setNeuNoiseToZero) {
644
645 const vector<t_pppParam*> &params = _parlist.params();
646 unsigned nPar = params.size();
647
648 _QFlt.ReSize(nPar);
649 _QFlt = 0.0;
650 _xFlt.ReSize(nPar);
651 _xFlt = 0.0;
652 _x0.ReSize(nPar);
653 _x0 = 0.0;
654
655 for (unsigned ii = 0; ii < nPar; ii++) {
656 t_pppParam *par1 = params[ii];
657 if (QFltOld.size() == 0) {
658 par1->resetIndex();
659 }
660 _x0[ii] = par1->x0();
661 int iOld = par1->indexOld();
662 if (iOld < 0) {
663 _QFlt[ii][ii] = par1->sigma0() * par1->sigma0(); // new parameter
664 } else {
665 _QFlt[ii][ii] = QFltOld[iOld][iOld] + par1->noise() * par1->noise();
666 _xFlt[ii] = xFltOld[iOld];
667 for (unsigned jj = 0; jj < ii; jj++) {
668 t_pppParam *par2 = params[jj];
669 int jOld = par2->indexOld();
670 if (jOld >= 0) {
671 _QFlt[ii][jj] = QFltOld(iOld + 1, jOld + 1);
672 }
673 }
674 }
675 }
676 predictCovCrdPart(QFltOld, setNeuNoiseToZero);
677}
678
679// Compute datum transformation
680////////////////////////////////////////////////////////////////////////////
681t_irc t_pppFilter::datumTransformation(
682 const QMap<char, t_pppRefSat*> &refSatMap) {
683
684 // get last epoch
685 t_pppObsPool::t_epoch *epoch = _obsPool->lastEpoch();
686 if (!epoch) {
687 LOG << "t_pppFilter::datumTransformation: !lastEpoch" << endl;
688 return failure;
689 }
690 _epoTime = epoch->epoTime();
691 LOG.setf(ios::fixed);
692 LOG << string(_epoTime) << "\nDATUM TRANSFORMATION " << 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
826 return success;
827}
828
829// Init datum transformation
830////////////////////////////////////////////////////////////////////////////
831void t_pppFilter::initDatumTransformation(
832 const std::vector<t_pppSatObs*> &allObs, bool pseudoObsIono) {
833 unsigned trafoObs = 0;
834 const QList<char> &usedSystems = _parlist.usedSystems();
835 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
836 char sys = usedSystems[iSys];
837 int satNum = 0;
838 for (unsigned jj = 0; jj < allObs.size(); jj++) {
839 if (allObs[jj]->prn().system() == sys) {
840 satNum++;
841 }
842 }
843 // all LCs
844 unsigned realUsedLCs = OPT->LCs(sys).size();
845 // exclude pseudo obs GIM
846 if (OPT->_pseudoObsIono && !pseudoObsIono) {
847 realUsedLCs -= 1;
848 }
849
850 trafoObs += satNum * realUsedLCs;
851
852 if (OPT->_pseudoObsIono && pseudoObsIono) {
853 trafoObs -= 1; // pseudo obs iono with respect to refSat
854 }
855 }
856 _datumTrafo->setNumObs(trafoObs);
857 _datumTrafo->setNumPar(_parlist.nPar());
858 _datumTrafo->initAA();
859}
860
861//
862//////////////////////////////////////////////////////////////////////////////
863bool t_pppFilter::resetRefSatellitesLastEpoch(
864 std::vector<t_pppSatObs*> &obsVector,
865 const QMap<char, t_pppRefSat*> &refSatMap,
866 const QMap<char, t_pppRefSat*> &refSatMapLastEpoch) {
867 bool resetRefSat;
868 // reference satellite definition per system
869 const QList<char> &usedSystems = _parlist.usedSystems();
870 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
871 resetRefSat = false;
872 char sys = usedSystems[iSys];
873 t_prn newPrn = refSatMap[sys]->prn();
874 t_prn oldPrn = refSatMapLastEpoch[sys]->prn();
875#ifdef BNC_DEBUG_PPP
876 if (oldPrn != newPrn) {
877 LOG << "oldRef: " << oldPrn.toString() << " => newRef " << newPrn.toString() << endl;
878 }
879#endif
880 vector<t_pppSatObs*>::iterator it = obsVector.begin();
881 while (it != obsVector.end()) {
882 t_pppSatObs *satObs = *it;
883 if (satObs->prn() == newPrn) {
884 resetRefSat = true;
885 satObs->setAsReference();
886 } else if (satObs->prn() == oldPrn) {
887 satObs->resetReference();
888 }
889 it++;
890 }
891 if (!resetRefSat) {
892 _obsPool->setRefSatChangeRequired(sys, true);
893 return resetRefSat;
894 }
895 }
896 return resetRefSat;
897}
Note: See TracBrowser for help on using the repository browser.