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

Last change on this file since 6128 was 6107, checked in by mervart, 10 years ago
File size: 11.0 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::type tLC,
37 const vector<t_pppSatObs*>* obsVector) {
38
39 _type = type;
40 _prn = prn;
41 _tLC = tLC;
42 _x0 = 0.0;
43 _indexOld = -1;
44 _indexNew = -1;
45 _noise = 0.0;
46 _ambInfo = 0;
47
48 switch (_type) {
49 case crdX:
50 _epoSpec = false;
51 _sigma0 = OPT->_aprSigCrd[0];
52 _noise = OPT->_noiseCrd[0];
53 break;
54 case crdY:
55 _epoSpec = false;
56 _sigma0 = OPT->_aprSigCrd[1];
57 _noise = OPT->_noiseCrd[1];
58 break;
59 case crdZ:
60 _epoSpec = false;
61 _sigma0 = OPT->_aprSigCrd[2];
62 _noise = OPT->_noiseCrd[2];
63 break;
64 case clkR:
65 _epoSpec = true;
66 _sigma0 = OPT->_noiseClk;
67 break;
68 case amb:
69 _epoSpec = false;
70 _sigma0 = OPT->_aprSigAmb;
71 _ambInfo = new t_ambInfo();
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 double offGG = 0;
77 if (_prn.system() == 'R' && tLC != t_lc::MW) {
78 offGG = PPP_CLIENT->offGG();
79 }
80 _x0 = floor((obs->obsValue(tLC) - offGG - obs->cmpValue(tLC)) / obs->lambda(tLC) + 0.5);
81 break;
82 }
83 }
84 }
85 break;
86 case offGG:
87 _epoSpec = true;
88 _sigma0 = 1000.0;
89 _x0 = PPP_CLIENT->offGG();
90 break;
91 case trp:
92 _epoSpec = false;
93 _sigma0 = OPT->_aprSigTrp;
94 _noise = OPT->_noiseTrp;
95 break;
96 }
97}
98
99// Destructor
100////////////////////////////////////////////////////////////////////////////
101t_pppParam::~t_pppParam() {
102 delete _ambInfo;
103}
104
105//
106////////////////////////////////////////////////////////////////////////////
107double t_pppParam::partial(const bncTime& /* epoTime */, const t_pppSatObs* obs,
108 const t_lc::type& tLC) const {
109
110 // Special Case - Melbourne-Wuebbena
111 // ---------------------------------
112 if (tLC == t_lc::MW && _type != amb) {
113 return 0.0;
114 }
115
116 const t_pppStation* sta = PPP_CLIENT->staRover();
117 ColumnVector rhoV = sta->xyzApr() - obs->xc().Rows(1,3);
118
119 switch (_type) {
120 case crdX:
121 return (sta->xyzApr()[0] - obs->xc()[0]) / rhoV.norm_Frobenius();
122 case crdY:
123 return (sta->xyzApr()[1] - obs->xc()[1]) / rhoV.norm_Frobenius();
124 case crdZ:
125 return (sta->xyzApr()[2] - obs->xc()[2]) / rhoV.norm_Frobenius();
126 case clkR:
127 return 1.0;
128 case offGG:
129 return (obs->prn().system() == 'R') ? 1.0 : 0.0;
130 case amb:
131 if (obs->prn() == _prn) {
132 if (tLC == _tLC) {
133 return (obs->lambda(tLC));
134 }
135 else if (tLC == t_lc::lIF && _tLC == t_lc::MW) {
136 return obs->lambda(t_lc::lIF) * obs->lambda(t_lc::MW) / obs->lambda(t_lc::l2);
137 }
138 else {
139 map<t_frequency::type, double> codeCoeff;
140 map<t_frequency::type, double> phaseCoeff;
141 obs->lcCoeff(tLC, codeCoeff, phaseCoeff);
142 if (_tLC == t_lc::l1) {
143 return obs->lambda(t_lc::l1) * phaseCoeff[t_lc::toFreq(obs->prn().system(),t_lc::l1)];
144 }
145 else if (_tLC == t_lc::l2) {
146 return obs->lambda(t_lc::l2) * phaseCoeff[t_lc::toFreq(obs->prn().system(),t_lc::l2)];
147 }
148 }
149 }
150 return 0.0;
151 case trp:
152 return 1.0 / sin(obs->eleSat());
153 }
154
155 return 0.0;
156}
157
158//
159////////////////////////////////////////////////////////////////////////////
160string t_pppParam::toString() const {
161 stringstream ss;
162 switch (_type) {
163 case crdX:
164 ss << "CRD_X";
165 break;
166 case crdY:
167 ss << "CRD_Y";
168 break;
169 case crdZ:
170 ss << "CRD_Z";
171 break;
172 case clkR:
173 ss << "CLK ";
174 break;
175 case amb:
176 ss << "AMB " << left << setw(3) << t_lc::toString(_tLC) << right << ' ' << _prn.toString();
177 break;
178 case offGG:
179 ss << "OGG ";
180 break;
181 case trp:
182 ss << "TRP ";
183 break;
184 }
185 return ss.str();
186}
187
188// Constructor
189////////////////////////////////////////////////////////////////////////////
190t_pppParlist::t_pppParlist() {
191}
192
193// Destructor
194////////////////////////////////////////////////////////////////////////////
195t_pppParlist::~t_pppParlist() {
196 for (unsigned ii = 0; ii < _params.size(); ii++) {
197 delete _params[ii];
198 }
199}
200
201//
202////////////////////////////////////////////////////////////////////////////
203t_irc t_pppParlist::set(const bncTime& epoTime, const std::vector<t_pppSatObs*>& obsVector) {
204
205 // Remove some Parameters
206 // ----------------------
207 vector<t_pppParam*>::iterator it = _params.begin();
208 while (it != _params.end()) {
209 t_pppParam* par = *it;
210
211 bool remove = false;
212
213 if (par->epoSpec()) {
214 remove = true;
215 }
216
217 else if (par->type() == t_pppParam::amb) {
218 if (par->lastObsTime().valid() && (epoTime - par->lastObsTime() > 120.0)) {
219 remove = true;
220 }
221 }
222
223 else if (par->type() == t_pppParam::amb) {
224 if (par->lastObsTime().valid() && (epoTime - par->lastObsTime() > 3600.0)) {
225 remove = true;
226 }
227 }
228
229 if (remove) {
230 delete par;
231 it = _params.erase(it);
232 }
233 else {
234 ++it;
235 }
236 }
237
238 // Check whether parameters have observations
239 // ------------------------------------------
240 for (unsigned ii = 0; ii < _params.size(); ii++) {
241 t_pppParam* par = _params[ii];
242 if (par->prn() == 0) {
243 par->setLastObsTime(epoTime);
244 if (par->firstObsTime().undef()) {
245 par->setFirstObsTime(epoTime);
246 }
247 }
248 else {
249 for (unsigned jj = 0; jj < obsVector.size(); jj++) {
250 const t_pppSatObs* satObs = obsVector[jj];
251 if (satObs->prn() == par->prn()) {
252 par->setLastObsTime(epoTime);
253 if (par->firstObsTime().undef()) {
254 par->setFirstObsTime(epoTime);
255 }
256 break;
257 }
258 }
259 }
260 }
261
262 // Required Set of Parameters
263 // --------------------------
264 vector<t_pppParam*> required;
265
266 // Coordinates
267 // -----------
268 required.push_back(new t_pppParam(t_pppParam::crdX, t_prn(), t_lc::dummy));
269 required.push_back(new t_pppParam(t_pppParam::crdY, t_prn(), t_lc::dummy));
270 required.push_back(new t_pppParam(t_pppParam::crdZ, t_prn(), t_lc::dummy));
271
272 // Receiver Clock
273 // --------------
274 required.push_back(new t_pppParam(t_pppParam::clkR, t_prn(), t_lc::dummy));
275
276 // GPS-Glonass Clock Offset
277 // ------------------------
278 if (OPT->useSystem('R')) {
279 required.push_back(new t_pppParam(t_pppParam::offGG, t_prn(), t_lc::dummy));
280 }
281
282 // Troposphere
283 // -----------
284 if (OPT->estTrp()) {
285 required.push_back(new t_pppParam(t_pppParam::trp, t_prn(), t_lc::dummy));
286 }
287
288 // Ambiguities
289 // -----------
290 for (unsigned jj = 0; jj < obsVector.size(); jj++) {
291 const t_pppSatObs* satObs = obsVector[jj];
292 const vector<t_lc::type>& ambLCs = OPT->ambLCs(satObs->prn().system());
293 for (unsigned ii = 0; ii < ambLCs.size(); ii++) {
294 required.push_back(new t_pppParam(t_pppParam::amb, satObs->prn(), ambLCs[ii], &obsVector));
295 }
296 }
297
298 // Check if all required parameters are present
299 // --------------------------------------------
300 for (unsigned ii = 0; ii < required.size(); ii++) {
301 t_pppParam* parReq = required[ii];
302
303 bool found = false;
304 for (unsigned jj = 0; jj < _params.size(); jj++) {
305 t_pppParam* parOld = _params[jj];
306 if (parOld->isEqual(parReq)) {
307 found = true;
308 break;
309 }
310 }
311 if (found) {
312 delete parReq;
313 }
314 else {
315 _params.push_back(parReq);
316 }
317 }
318
319 // Set Parameter Indices
320 // ---------------------
321 sort(_params.begin(), _params.end(), t_pppParam::sortFunction);
322
323 for (unsigned ii = 0; ii < _params.size(); ii++) {
324 t_pppParam* par = _params[ii];
325 par->setIndex(ii);
326 for (unsigned jj = 0; jj < obsVector.size(); jj++) {
327 const t_pppSatObs* satObs = obsVector[jj];
328 if (satObs->prn() == par->prn()) {
329 par->setAmbEleSat(satObs->eleSat());
330 par->stepAmbNumEpo();
331 }
332 }
333 }
334
335 return success;
336}
337
338//
339////////////////////////////////////////////////////////////////////////////
340void t_pppParlist::printResult(const bncTime& epoTime, const SymmetricMatrix& QQ,
341 const ColumnVector& xx) const {
342
343 string epoTimeStr = string(epoTime);
344
345 LOG << endl;
346
347 t_pppParam* parX = 0;
348 t_pppParam* parY = 0;
349 t_pppParam* parZ = 0;
350 for (unsigned ii = 0; ii < _params.size(); ii++) {
351 t_pppParam* par = _params[ii];
352 if (par->type() == t_pppParam::crdX) {
353 parX = par;
354 }
355 else if (par->type() == t_pppParam::crdY) {
356 parY = par;
357 }
358 else if (par->type() == t_pppParam::crdZ) {
359 parZ = par;
360 }
361 else {
362 int ind = par->indexNew();
363 LOG << epoTimeStr << ' ' << par->toString() << ' '
364 << setw(10) << setprecision(4) << par->x0() << ' '
365 << showpos << setw(10) << setprecision(4) << xx[ind] << noshowpos << " +- "
366 << setw(8) << setprecision(4) << sqrt(QQ[ind][ind]);
367 if (par->type() == t_pppParam::amb) {
368 LOG << " el = " << setw(6) << setprecision(2) << par->ambEleSat() * 180.0 / M_PI
369 << " epo = " << setw(4) << par->ambNumEpo();
370 }
371 LOG << endl;
372 }
373 }
374
375 if (parX && parY && parZ) {
376 const t_pppStation* sta = PPP_CLIENT->staRover();
377
378 ColumnVector xyz(3);
379 xyz[0] = xx[parX->indexNew()];
380 xyz[1] = xx[parY->indexNew()];
381 xyz[2] = xx[parZ->indexNew()];
382
383 ColumnVector neu(3);
384 xyz2neu(sta->ellApr().data(), xyz.data(), neu.data());
385
386 SymmetricMatrix QQxyz = QQ.SymSubMatrix(1,3);
387
388 SymmetricMatrix QQneu(3);
389 covariXYZ_NEU(QQxyz, sta->ellApr().data(), QQneu);
390
391 LOG << epoTimeStr << ' ' << sta->name()
392 << " X = " << setprecision(4) << sta->xyzApr()[0] + xyz[0] << " +- "
393 << setprecision(4) << sqrt(QQxyz[0][0])
394
395 << " Y = " << setprecision(4) << sta->xyzApr()[1] + xyz[1] << " +- "
396 << setprecision(4) << sqrt(QQxyz[1][1])
397
398 << " Z = " << setprecision(4) << sta->xyzApr()[2] + xyz[2] << " +- "
399 << setprecision(4) << sqrt(QQxyz[2][2])
400
401 << " dN = " << setprecision(4) << neu[0] << " +- "
402 << setprecision(4) << sqrt(QQneu[0][0])
403
404 << " dE = " << setprecision(4) << neu[1] << " +- "
405 << setprecision(4) << sqrt(QQneu[1][1])
406
407 << " dU = " << setprecision(4) << neu[2] << " +- "
408 << setprecision(4) << sqrt(QQneu[2][2])
409
410 << endl;
411 }
412}
413
Note: See TracBrowser for help on using the repository browser.