source: ntrip/trunk/BNC/src/PPP/pppParlist.cpp@ 10793

Last change on this file since 10793 was 10791, checked in by mervart, 2 weeks ago

BNC Multifrequency and PPPAR Client (initial version)
  • Property svn:keywords set to Author Date Id Rev URL;svn:eol-style=native
  • Property svn:mime-type set to text/plain
File size: 13.7 KB
Line 
1/* -------------------------------------------------------------------------
2 * BKG NTRIP Client
3 * -------------------------------------------------------------------------
4 *
5 * Class: t_pppParlist
6 *
7 * Purpose: List of estimated parameters
8 *
9 * Author: L. Mervart
10 *
11 * Created: 29-Jul-2014
12 *
13 * Changes:
14 *
15 * -----------------------------------------------------------------------*/
16
17#include <cmath>
18#include <iostream>
19#include <sstream>
20#include <iomanip>
21#include <algorithm>
22#include <newmatio.h>
23
24#include "pppParlist.h"
25#include "pppSatObs.h"
26#include "pppStation.h"
27#include "bncutils.h"
28#include "bncconst.h"
29#include "pppClient.h"
30
31using namespace BNC_PPP;
32using namespace std;
33
34// Constructor
35////////////////////////////////////////////////////////////////////////////
36t_pppParam::t_pppParam(e_type type, const t_prn& prn, t_lc LC,
37 const vector<t_pppSatObs*>* obsVector) {
38
39 _type = type;
40 _prn = prn;
41 _sys = '?';
42 _LC = LC;
43 _x0 = 0.0;
44 _indexOld = -1;
45 _indexNew = -1;
46 _noise = 0.0;
47 _ambInfo = new t_ambInfo();
48
49 switch (_type) {
50 case crdX:
51 _epoSpec = false;
52 _sigma0 = OPT->_aprSigCrd[0];
53 _noise = OPT->_noiseCrd[0];
54 break;
55 case crdY:
56 _epoSpec = false;
57 _sigma0 = OPT->_aprSigCrd[1];
58 _noise = OPT->_noiseCrd[1];
59 break;
60 case crdZ:
61 _epoSpec = false;
62 _sigma0 = OPT->_aprSigCrd[2];
63 _noise = OPT->_noiseCrd[2];
64 break;
65 case rClk:
66 _epoSpec = true;
67 _sigma0 = OPT->_aprSigClk;
68 break;
69 case amb:
70 _epoSpec = false;
71 _sigma0 = OPT->_aprSigAmb;
72 if (obsVector) {
73 for (unsigned ii = 0; ii < obsVector->size(); ii++) {
74 const t_pppSatObs* obs = obsVector->at(ii);
75 if (obs->prn() == _prn) {
76 _x0 = floor((obs->obsValue(LC) - obs->cmpValue(LC)) / obs->lambda(LC) + 0.5);
77 break;
78 }
79 }
80 }
81 break;
82 case trp:
83 _epoSpec = false;
84 _sigma0 = OPT->_aprSigTrp;
85 _noise = OPT->_noiseTrp;
86 break;
87 case ion:
88 _epoSpec = true;
89 _sigma0 = OPT->_aprSigIon;
90 break;
91 case bias:
92 _epoSpec = true;
93 _sigma0 = OPT->_aprSigCodeBias;
94 break;
95 }
96}
97
98// Destructor
99////////////////////////////////////////////////////////////////////////////
100t_pppParam::~t_pppParam() {
101 delete _ambInfo;
102}
103//
104////////////////////////////////////////////////////////////////////////////
105double t_pppParam::partial(const bncTime& /* epoTime */, const t_pppSatObs* obs,
106 const t_lc& obsLC) const {
107
108 // Special Case - Melbourne-Wuebbena
109 // ---------------------------------
110 if (obsLC._type == t_lc::MW && _type != amb) {
111 return 0.0;
112 }
113
114 // Special Case - GIM
115 // ------------------
116 if (obsLC._type == t_lc::GIM) {
117 if (_type == ion) {
118 return 1.0;
119 }
120 else {
121 return 0.0;
122 }
123 }
124
125 const t_pppStation* sta = PPP_CLIENT->staRover();
126 ColumnVector rhoV = sta->xyzApr() - obs->xc().Rows(1,3);
127
128 map<t_frequency::type, double> codeCoeff;
129 map<t_frequency::type, double> phaseCoeff;
130 map<t_frequency::type, double> ionoCoeff;
131 obs->lcCoeff(obsLC, codeCoeff, phaseCoeff, ionoCoeff);
132
133 switch (_type) {
134 case crdX:
135 return (sta->xyzApr()[0] - obs->xc()[0]) / rhoV.NormFrobenius();
136 case crdY:
137 return (sta->xyzApr()[1] - obs->xc()[1]) / rhoV.NormFrobenius();
138 case crdZ:
139 return (sta->xyzApr()[2] - obs->xc()[2]) / rhoV.NormFrobenius();
140 case rClk:
141 return (_sys != obsLC.system() || obsLC.isGeometryFree()) ? 0.0 : 1.0;
142 case amb:
143 if (obs->prn() == _prn) {
144 if (obsLC == _LC) {
145 return (obs->lambda(obsLC));
146 }
147 else if (_LC._type == t_lc::phase) {
148 return obs->lambda(_LC) * phaseCoeff[_LC._frq1];
149 }
150 else if (obsLC._type == t_lc::phaseIF && _LC._type == t_lc::MW) {
151 return obs->lambda(obsLC) * obs->lambda(_LC) / obs->lambda(t_lc(t_lc::phase, _LC._frq2));
152 }
153 }
154 break;
155 case trp:
156 return 1.0 / sin(obs->eleSat());
157 case ion:
158 if (obs->prn() == _prn) {
159 if (obsLC._type == t_lc::code || obsLC._type == t_lc::phase) {
160 return ionoCoeff[obsLC._frq1];
161 }
162 }
163 break;
164 case bias:
165 return (_LC == obsLC ? 1.0 : 0.0);
166 }
167 return 0.0;
168}
169
170//
171////////////////////////////////////////////////////////////////////////////
172string t_pppParam::toString() const {
173 stringstream ss;
174 switch (_type) {
175 case crdX:
176 ss << "CRD_X";
177 break;
178 case crdY:
179 ss << "CRD_Y";
180 break;
181 case crdZ:
182 ss << "CRD_Z";
183 break;
184 case rClk:
185 ss << "REC_CLK " << _sys << " ";
186 break;
187 case trp:
188 ss << "TRP ";
189 break;
190 case amb:
191 ss << "AMB " << left << setw(3) << _LC.toString() << right << ' ' << _prn.toString();
192 break;
193 case ion:
194 ss << "ION " << left << setw(3) << _LC.toString() << right << ' ' << _prn.toString();
195 break;
196 case bias:
197 char sys = t_frequency::toSystem(_LC._frq1);
198 ss << "BIA " << left << setw(3) << _LC.toString() << right << ' ' << sys << " ";
199 break;
200 }
201 return ss.str();
202}
203
204// Constructor
205////////////////////////////////////////////////////////////////////////////
206t_pppParlist::t_pppParlist() {
207}
208
209// Destructor
210////////////////////////////////////////////////////////////////////////////
211t_pppParlist::~t_pppParlist() {
212
213 for (unsigned ii = 0; ii < _params.size(); ii++) {
214 delete _params[ii];
215 }
216}
217
218//
219////////////////////////////////////////////////////////////////////////////
220t_irc t_pppParlist::set(const bncTime& epoTime, const std::vector<t_pppSatObs*>& obsVector) {
221
222 // Remove some Parameters
223 // ----------------------
224 vector<t_pppParam*>::iterator it = _params.begin();
225 while (it != _params.end()) {
226 t_pppParam* par = *it;
227
228 bool remove = false;
229
230 if (par->epoSpec()) {
231 remove = true;
232 }
233
234 else if (par->type() == t_pppParam::amb ||
235 par->type() == t_pppParam::crdX ||
236 par->type() == t_pppParam::crdY ||
237 par->type() == t_pppParam::crdZ ||
238 par->type() == t_pppParam::ion ) {
239 if (par->lastObsTime().valid() && (epoTime - par->lastObsTime() > 60.0)) {
240 remove = true;
241 }
242 }
243
244 if (remove) {
245#ifdef BNC_DEBUG_PPP
246// LOG << "remove0 " << par->toString() << std::endl;
247#endif
248 delete par;
249 it = _params.erase(it);
250 }
251 else {
252 ++it;
253 }
254 }
255
256 // Check which systems have observations
257 // -------------------------------------
258 vector<char> systems = OPT->systems();
259 for (char sys : systems) {
260 _usedSystems[sys] = 0;
261 for (unsigned jj = 0; jj < obsVector.size(); jj++) {
262 const t_pppSatObs* satObs = obsVector[jj];
263 if (satObs->prn().system() == sys) {
264 _usedSystems[sys]++;
265 }
266 }
267 };
268
269 // Check whether parameters have observations
270 // ------------------------------------------
271 for (unsigned ii = 0; ii < _params.size(); ii++) {
272 t_pppParam* par = _params[ii];
273 if (par->prn() == 0) {
274 par->setLastObsTime(epoTime);
275 if (par->firstObsTime().undef()) {
276 par->setFirstObsTime(epoTime);
277 }
278 }
279 else {
280 for (unsigned jj = 0; jj < obsVector.size(); jj++) {
281 const t_pppSatObs* satObs = obsVector[jj];
282 if (satObs->prn() == par->prn()) {
283 par->setLastObsTime(epoTime);
284 if (par->firstObsTime().undef()) {
285 par->setFirstObsTime(epoTime);
286 }
287 break;
288 }
289 }
290 }
291 }
292
293 // Required Set of Parameters
294 // --------------------------
295 vector<t_pppParam*> required;
296
297 // Coordinates
298 // -----------
299 required.push_back(new t_pppParam(t_pppParam::crdX, t_prn()));
300 required.push_back(new t_pppParam(t_pppParam::crdY, t_prn()));
301 required.push_back(new t_pppParam(t_pppParam::crdZ, t_prn()));
302
303 // Receiver Clocks
304 // ---------------
305 for (const auto& [sys, numObs] : _usedSystems) {
306 if (numObs > 0) {
307 t_pppParam* clk = new t_pppParam(t_pppParam::rClk, t_prn());
308 clk->setSystem(sys);
309 required.push_back(clk);
310 }
311 }
312
313 // Troposphere
314 // -----------
315 if (OPT->estTrp()) {
316 required.push_back(new t_pppParam(t_pppParam::trp, t_prn()));
317 }
318
319 // Ionosphere
320 // ----------
321 if (OPT->_ionoModelType == OPT->est) {
322 for (unsigned jj = 0; jj < obsVector.size(); jj++) {
323 const t_pppSatObs* satObs = obsVector[jj];
324 std::vector<t_lc> LCs = OPT->LCs(satObs->prn().system());
325 for (auto it = LCs.begin(); it != LCs.end(); ++it) {
326 const t_lc& lc = *it;
327 if (!lc.isIonoFree()) {
328 required.push_back(new t_pppParam(t_pppParam::ion, satObs->prn()));
329 break;
330 }
331 }
332 }
333 }
334
335 // Ambiguities
336 // -----------
337 for (unsigned jj = 0; jj < obsVector.size(); jj++) {
338 const t_pppSatObs* satObs = obsVector[jj];
339 char sys = satObs->prn().system();
340 const vector<t_lc>& ambLCs = OPT->ambLCs(sys);
341 for (unsigned ii = 0; ii < ambLCs.size(); ii++) {
342 if (ambLCs[ii]._frq1 == t_frequency::G5 && !satObs->isValid(ambLCs[ii])) {
343 continue;
344 }
345 required.push_back(new t_pppParam(t_pppParam::amb, satObs->prn(), ambLCs[ii], &obsVector));
346 }
347 }
348
349 // Biases
350 // ------
351 int maxSkip = (OPT->_pseudoObsIono ? 1 : 2);
352 for (const auto& [sys, numObs] : _usedSystems) {
353 if (numObs > 0) {
354 bool ar = OPT->arSystem(sys);
355 int skip = 0;
356 vector<t_lc> LCs = OPT->LCs(sys);
357 for (const t_lc& lc : LCs) {
358 if (ar) {
359 if (skip < maxSkip && lc.includesPhase()) {
360 skip += 1;
361 }
362 else {
363 required.push_back(new t_pppParam(t_pppParam::bias, t_prn(), lc));
364 }
365 }
366 else {
367 if (lc.includesCode()) {
368 if (skip < maxSkip) {
369 skip += 1;
370 }
371 else {
372 required.push_back(new t_pppParam(t_pppParam::bias, t_prn(), lc));
373 }
374 }
375 }
376 }
377 }
378 }
379
380 // Check if all required parameters are present
381 // --------------------------------------------
382 for (unsigned ii = 0; ii < required.size(); ii++) {
383 t_pppParam* parReq = required[ii];
384
385 bool found = false;
386 for (unsigned jj = 0; jj < _params.size(); jj++) {
387 t_pppParam* parOld = _params[jj];
388 if (parOld->isEqual(parReq)) {
389 found = true;
390 break;
391 }
392 }
393 if (found) {
394 delete parReq;
395 }
396 else {
397 _params.push_back(parReq);
398 }
399 }
400
401 // Set Parameter Indices
402 // ---------------------
403 sort(_params.begin(), _params.end(), t_pppParam::sortFunction);
404
405 for (unsigned ii = 0; ii < _params.size(); ii++) {
406 t_pppParam* par = _params[ii];
407 par->setIndex(ii);
408 for (unsigned jj = 0; jj < obsVector.size(); jj++) {
409 const t_pppSatObs* satObs = obsVector[jj];
410 if (satObs->prn() == par->prn()) {
411 par->setAmbEleSat(satObs->eleSat());
412 par->stepAmbNumEpo();
413 }
414 }
415 }
416
417 return success;
418}
419
420//
421////////////////////////////////////////////////////////////////////////////
422void t_pppParlist::printResult(const bncTime& epoTime, const SymmetricMatrix& QQ,
423 const ColumnVector& xx, double fixRatio) const {
424
425 string epoTimeStr = string(epoTime);
426 const t_pppStation* sta = PPP_CLIENT->staRover();
427
428 LOG << endl;
429
430 t_pppParam* parX = 0;
431 t_pppParam* parY = 0;
432 t_pppParam* parZ = 0;
433 for (unsigned ii = 0; ii < _params.size(); ii++) {
434 t_pppParam* par = _params[ii];
435 if (par->type() == t_pppParam::crdX) {
436 parX = par;
437 }
438 else if (par->type() == t_pppParam::crdY) {
439 parY = par;
440 }
441 else if (par->type() == t_pppParam::crdZ) {
442 parZ = par;
443 }
444 else {
445 int ind = par->indexNew();
446 double apr = (par->type() == t_pppParam::trp) ?
447 t_tropo::delay_saast(sta->xyzApr(), M_PI/2.0) : par->x0();
448 LOG << epoTimeStr << ' ' << par->toString() << ' '
449 << setw(10) << setprecision(4) << apr << ' '
450 << showpos << setw(10) << setprecision(4) << xx[ind] << noshowpos << " +- "
451 << setw(8) << setprecision(4) << sqrt(QQ[ind][ind]);
452 if (par->type() == t_pppParam::amb) {
453 LOG << " el = " << setw(6) << setprecision(2) << par->ambEleSat() * 180.0 / M_PI
454 << " epo = " << setw(4) << par->ambNumEpo();
455 }
456 LOG << endl;
457 }
458 }
459
460 if (parX && parY && parZ) {
461
462 ColumnVector xyz(3);
463 xyz[0] = xx[parX->indexNew()];
464 xyz[1] = xx[parY->indexNew()];
465 xyz[2] = xx[parZ->indexNew()];
466
467 ColumnVector neu(3);
468 xyz2neu(sta->ellApr().data(), xyz.data(), neu.data());
469
470 SymmetricMatrix QQxyz = QQ.SymSubMatrix(1,3);
471
472 SymmetricMatrix QQneu(3);
473 covariXYZ_NEU(QQxyz, sta->ellApr().data(), QQneu);
474
475 LOG << epoTimeStr << ' ' << sta->name()
476 << " X = " << setprecision(4) << sta->xyzApr()[0] + xyz[0] << " +- "
477 << setprecision(4) << sqrt(QQxyz[0][0])
478
479 << " Y = " << setprecision(4) << sta->xyzApr()[1] + xyz[1] << " +- "
480 << setprecision(4) << sqrt(QQxyz[1][1])
481
482 << " Z = " << setprecision(4) << sta->xyzApr()[2] + xyz[2] << " +- "
483 << setprecision(4) << sqrt(QQxyz[2][2])
484
485 << " dN = " << setprecision(4) << neu[0] << " +- "
486 << setprecision(4) << sqrt(QQneu[0][0])
487
488 << " dE = " << setprecision(4) << neu[1] << " +- "
489 << setprecision(4) << sqrt(QQneu[1][1])
490
491 << " dU = " << setprecision(4) << neu[2] << " +- "
492 << setprecision(4) << sqrt(QQneu[2][2]);
493
494 if (fixRatio > 0.0) {
495 LOG << " fix " << int(100*fixRatio) << " %";
496 }
497 else {
498 LOG << " flt ";
499 }
500
501 LOG << endl;
502 }
503 return;
504}
505
506//
507////////////////////////////////////////////////////////////////////////////
508void t_pppParlist::printParams(const bncTime& epoTime) {
509
510 for (unsigned iPar = 0; iPar < _params.size(); iPar++) {
511 LOG << _params[iPar]->toString()
512 << "\t lastObsTime().valid() \t" << _params[iPar]->lastObsTime().valid()
513 << "\t epoTime - par->lastObsTime() \t" << (epoTime - _params[iPar]->lastObsTime())
514 << endl;
515 }
516}
517
Note: See TracBrowser for help on using the repository browser.