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

Last change on this file since 5832 was 5828, checked in by mervart, 11 years ago
File size: 12.4 KB
Line 
1
2// Part of BNC, a utility for retrieving decoding and
3// converting GNSS data streams from NTRIP broadcasters.
4//
5// Copyright (C) 2007
6// German Federal Agency for Cartography and Geodesy (BKG)
7// http://www.bkg.bund.de
8// Czech Technical University Prague, Department of Geodesy
9// http://www.fsv.cvut.cz
10//
11// Email: euref-ip@bkg.bund.de
12//
13// This program is free software; you can redistribute it and/or
14// modify it under the terms of the GNU General Public License
15// as published by the Free Software Foundation, version 2.
16//
17// This program is distributed in the hope that it will be useful,
18// but WITHOUT ANY WARRANTY; without even the implied warranty of
19// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
20// GNU General Public License for more details.
21//
22// You should have received a copy of the GNU General Public License
23// along with this program; if not, write to the Free Software
24// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
25
26/* -------------------------------------------------------------------------
27 * BKG NTRIP Client
28 * -------------------------------------------------------------------------
29 *
30 * Class: t_pppParlist
31 *
32 * Purpose: List of estimated parameters
33 *
34 * Author: L. Mervart
35 *
36 * Created: 29-Jul-2014
37 *
38 * Changes:
39 *
40 * -----------------------------------------------------------------------*/
41
42#include <cmath>
43#include <iostream>
44#include <sstream>
45#include <iomanip>
46#include <algorithm>
47#include <newmatio.h>
48
49#include "pppParlist.h"
50#include "pppSatObs.h"
51#include "pppStation.h"
52#include "bncutils.h"
53#include "bncconst.h"
54#include "pppClient.h"
55
56using namespace BNC_PPP;
57using namespace std;
58
59// Constructor
60////////////////////////////////////////////////////////////////////////////
61t_pppParam::t_pppParam(e_type type, const t_prn& prn, t_lc::type tLC,
62 const vector<t_pppSatObs*>* obsVector) {
63
64 _type = type;
65 _prn = prn;
66 _tLC = tLC;
67 _x0 = 0.0;
68 _indexOld = -1;
69 _indexNew = -1;
70 _noise = 0.0;
71 _ambInfo = 0;
72
73 switch (_type) {
74 case crdX:
75 _epoSpec = true;
76 _sigma0 = OPT->_sigCrd[0];
77 break;
78 case crdY:
79 _epoSpec = true;
80 _sigma0 = OPT->_sigCrd[1];
81 break;
82 case crdZ:
83 _epoSpec = true;
84 _sigma0 = OPT->_sigCrd[2];
85 break;
86 case clkR:
87 _epoSpec = true;
88 _sigma0 = 1000.0;
89 break;
90 case amb:
91 _ambInfo = new t_ambInfo();
92 if (obsVector) {
93 for (unsigned ii = 0; ii < obsVector->size(); ii++) {
94 const t_pppSatObs* obs = obsVector->at(ii);
95 if (obs->prn() == _prn) {
96 double offGG = 0;
97 if (_prn.system() == 'R' && tLC != t_lc::MW) {
98 offGG = PPP_CLIENT->offGG();
99 }
100 _x0 = floor((obs->obsValue(tLC) - offGG - obs->cmpValue(tLC)) / obs->lambda(tLC) + 0.5);
101 break;
102 }
103 }
104 }
105 _epoSpec = false;
106 _sigma0 = 100.0;
107 break;
108 case offGG:
109 _epoSpec = true;
110 _sigma0 = 1000.0;
111 _x0 = PPP_CLIENT->offGG();
112 break;
113 case trp:
114 _epoSpec = false;
115 _sigma0 = OPT->_sigTropo;
116 _noise = OPT->_noiseTropo;
117 break;
118 case bias:
119 _epoSpec = true;
120 _sigma0 = 10.0;
121 break;
122 }
123}
124
125// Destructor
126////////////////////////////////////////////////////////////////////////////
127t_pppParam::~t_pppParam() {
128 delete _ambInfo;
129}
130
131//
132////////////////////////////////////////////////////////////////////////////
133double t_pppParam::partial(const bncTime& /* epoTime */, const t_pppSatObs* obs,
134 const t_lc::type& tLC) const {
135
136 // Special Case - Melbourne-Wuebbena
137 // ---------------------------------
138 if (tLC == t_lc::MW && _type != amb && _type != bias) {
139 return 0.0;
140 }
141
142 const t_pppStation* sta = PPP_CLIENT->staRover();
143 ColumnVector rhoV = sta->xyzApr() - obs->xc().Rows(1,3);
144
145 switch (_type) {
146 case crdX:
147 return (sta->xyzApr()[0] - obs->xc()[0]) / rhoV.norm_Frobenius();
148 case crdY:
149 return (sta->xyzApr()[1] - obs->xc()[1]) / rhoV.norm_Frobenius();
150 case crdZ:
151 return (sta->xyzApr()[2] - obs->xc()[2]) / rhoV.norm_Frobenius();
152 case clkR:
153 return 1.0;
154 case offGG:
155 return (obs->prn().system() == 'R') ? 1.0 : 0.0;
156 case amb:
157 if (obs->prn() == _prn) {
158 if (tLC == _tLC) {
159 return (obs->lambda(tLC));
160 }
161 else if (tLC == t_lc::lIF && _tLC == t_lc::MW) {
162 return obs->lambda(t_lc::lIF) * obs->lambda(t_lc::MW) / obs->lambda(t_lc::l2);
163 }
164 else {
165 ColumnVector coeff(4);
166 obs->lc(tLC, 0.0, 0.0, 0.0, 0.0, &coeff);
167 if (_tLC == t_lc::l1) {
168 return obs->lambda(t_lc::l1) * coeff(1);
169 }
170 else if (_tLC == t_lc::l2) {
171 return obs->lambda(t_lc::l2) * coeff(2);
172 }
173 }
174 }
175 return 0.0;
176 case trp:
177 return 1.0 / sin(obs->eleSat());
178 case bias:
179 if (tLC == _tLC && obs->prn().system() == _prn.system()) {
180 return 1.0;
181 }
182 else {
183 return 0.0;
184 }
185 }
186
187 return 0.0;
188}
189
190//
191////////////////////////////////////////////////////////////////////////////
192string t_pppParam::toString() const {
193 stringstream ss;
194 switch (_type) {
195 case crdX:
196 ss << "CRD_X";
197 break;
198 case crdY:
199 ss << "CRD_Y";
200 break;
201 case crdZ:
202 ss << "CRD_Z";
203 break;
204 case clkR:
205 ss << "CLK ";
206 break;
207 case amb:
208 ss << "AMB " << left << setw(3) << t_lc::toString(_tLC) << right << ' ' << _prn.toString();
209 break;
210 case offGG:
211 ss << "OGG ";
212 break;
213 case trp:
214 ss << "TRP ";
215 break;
216 case bias:
217 ss << "BIAS " << _prn.system() << ' ' << left << setw(3) << t_lc::toString(_tLC);
218 break;
219 }
220 return ss.str();
221}
222
223// Constructor
224////////////////////////////////////////////////////////////////////////////
225t_pppParlist::t_pppParlist() {
226}
227
228// Destructor
229////////////////////////////////////////////////////////////////////////////
230t_pppParlist::~t_pppParlist() {
231 for (unsigned ii = 0; ii < _params.size(); ii++) {
232 delete _params[ii];
233 }
234}
235
236//
237////////////////////////////////////////////////////////////////////////////
238t_irc t_pppParlist::set(const bncTime& epoTime, const vector<t_lc::type>& ambLCs,
239 const vector<t_pppSatObs*>& obsVector) {
240
241 // Remove some Parameters
242 // ----------------------
243 vector<t_pppParam*>::iterator it = _params.begin();
244 while (it != _params.end()) {
245 t_pppParam* par = *it;
246
247 bool remove = false;
248
249 if (par->epoSpec()) {
250 remove = true;
251 }
252
253 else if (par->type() == t_pppParam::amb) {
254 if (par->lastObsTime().valid() && (epoTime - par->lastObsTime() > 120.0)) {
255 remove = true;
256 }
257 }
258
259 else if (par->type() == t_pppParam::amb) {
260 if (par->lastObsTime().valid() && (epoTime - par->lastObsTime() > 3600.0)) {
261 remove = true;
262 }
263 }
264
265 if (remove) {
266 delete par;
267 it = _params.erase(it);
268 }
269 else {
270 ++it;
271 }
272 }
273
274 // Check whether parameters have observations
275 // ------------------------------------------
276 for (unsigned ii = 0; ii < _params.size(); ii++) {
277 t_pppParam* par = _params[ii];
278 if (par->prn() == 0) {
279 par->setLastObsTime(epoTime);
280 if (par->firstObsTime().undef()) {
281 par->setFirstObsTime(epoTime);
282 }
283 }
284 else {
285 for (unsigned jj = 0; jj < obsVector.size(); jj++) {
286 const t_pppSatObs* satObs = obsVector[jj];
287 if (satObs->prn() == par->prn()) {
288 par->setLastObsTime(epoTime);
289 if (par->firstObsTime().undef()) {
290 par->setFirstObsTime(epoTime);
291 }
292 break;
293 }
294 }
295 }
296 }
297
298 // Required Set of Parameters
299 // --------------------------
300 vector<t_pppParam*> required;
301
302 // Coordinates
303 // -----------
304 required.push_back(new t_pppParam(t_pppParam::crdX, t_prn(), t_lc::dummy));
305 required.push_back(new t_pppParam(t_pppParam::crdY, t_prn(), t_lc::dummy));
306 required.push_back(new t_pppParam(t_pppParam::crdZ, t_prn(), t_lc::dummy));
307
308 // Receiver Clock
309 // --------------
310 required.push_back(new t_pppParam(t_pppParam::clkR, t_prn(), t_lc::dummy));
311
312 // GPS-Glonass Clock Offset
313 // ------------------------
314 if (OPT->useGlonass()) {
315 required.push_back(new t_pppParam(t_pppParam::offGG, t_prn(), t_lc::dummy));
316 }
317
318 // Receiver Biases
319 // ---------------
320 for (unsigned ii = 0; ii < OPT->_estBias.size(); ii++) {
321 const t_pppOptions::t_optBias& optBias = OPT->_estBias[ii];
322 required.push_back(new t_pppParam(t_pppParam::bias, t_prn(optBias._system, 1), optBias._tLC));
323 }
324
325 // Troposphere
326 // -----------
327 if (OPT->estTropo()) {
328 required.push_back(new t_pppParam(t_pppParam::trp, t_prn(), t_lc::dummy));
329 }
330
331 // Ambiguities
332 // -----------
333 for (unsigned ii = 0; ii < ambLCs.size(); ii++) {
334 const t_lc::type& tLC = ambLCs[ii];
335 for (unsigned jj = 0; jj < obsVector.size(); jj++) {
336 const t_pppSatObs* satObs = obsVector[jj];
337 required.push_back(new t_pppParam(t_pppParam::amb, satObs->prn(), tLC, &obsVector));
338 }
339 }
340
341 // Check if all required parameters are present
342 // --------------------------------------------
343 for (unsigned ii = 0; ii < required.size(); ii++) {
344 t_pppParam* parReq = required[ii];
345
346 bool found = false;
347 for (unsigned jj = 0; jj < _params.size(); jj++) {
348 t_pppParam* parOld = _params[jj];
349 if (parOld->isEqual(parReq)) {
350 found = true;
351 break;
352 }
353 }
354 if (found) {
355 delete parReq;
356 }
357 else {
358 _params.push_back(parReq);
359 }
360 }
361
362 // Set Parameter Indices
363 // ---------------------
364 sort(_params.begin(), _params.end(), t_pppParam::sortFunction);
365
366 for (unsigned ii = 0; ii < _params.size(); ii++) {
367 t_pppParam* par = _params[ii];
368 par->setIndex(ii);
369 for (unsigned jj = 0; jj < obsVector.size(); jj++) {
370 const t_pppSatObs* satObs = obsVector[jj];
371 if (satObs->prn() == par->prn()) {
372 par->setAmbEleSat(satObs->eleSat());
373 par->stepAmbNumEpo();
374 }
375 }
376 }
377
378 return success;
379}
380
381//
382////////////////////////////////////////////////////////////////////////////
383void t_pppParlist::printResult(const bncTime& epoTime, const SymmetricMatrix& QQ,
384 const ColumnVector& xx, double ambFixRate) const {
385
386 string epoTimeStr = string(epoTime);
387
388 LOG << endl;
389
390 t_pppParam* parX = 0;
391 t_pppParam* parY = 0;
392 t_pppParam* parZ = 0;
393 for (unsigned ii = 0; ii < _params.size(); ii++) {
394 t_pppParam* par = _params[ii];
395 if (par->type() == t_pppParam::crdX) {
396 parX = par;
397 }
398 else if (par->type() == t_pppParam::crdY) {
399 parY = par;
400 }
401 else if (par->type() == t_pppParam::crdZ) {
402 parZ = par;
403 }
404 else {
405 int ind = par->indexNew();
406 LOG << epoTimeStr << ' ' << par->toString() << ' '
407 << setw(10) << setprecision(4) << par->x0() << ' '
408 << showpos << setw(10) << setprecision(4) << xx[ind] << noshowpos << " +- "
409 << setw(8) << setprecision(4) << sqrt(QQ[ind][ind]);
410 if (par->type() == t_pppParam::amb) {
411 LOG << " el = " << setw(6) << setprecision(2) << par->ambEleSat() * 180.0 / M_PI
412 << " epo = " << setw(4) << par->ambNumEpo();
413 }
414 LOG << endl;
415 }
416 }
417
418 if (parX && parY && parZ) {
419 const t_pppStation* sta = PPP_CLIENT->staRover();
420
421 ColumnVector xyz(3);
422 xyz[0] = xx[parX->indexNew()];
423 xyz[1] = xx[parY->indexNew()];
424 xyz[2] = xx[parZ->indexNew()];
425
426 ColumnVector neu(3);
427 xyz2neu(sta->ellApr().data(), xyz.data(), neu.data());
428
429 SymmetricMatrix QQxyz = QQ.SymSubMatrix(1,3);
430
431 SymmetricMatrix QQneu(3);
432 covariXYZ_NEU(QQxyz, sta->ellApr().data(), QQneu);
433
434 LOG << epoTimeStr
435 << " X = " << setprecision(4) << sta->xyzApr()[0] + xyz[0] << " +- "
436 << setprecision(4) << sqrt(QQxyz[0][0])
437
438 << " Y = " << setprecision(4) << sta->xyzApr()[1] + xyz[1] << " +- "
439 << setprecision(4) << sqrt(QQxyz[1][1])
440
441 << " Z = " << setprecision(4) << sta->xyzApr()[2] + xyz[2] << " +- "
442 << setprecision(4) << sqrt(QQxyz[2][2])
443
444 << " dN = " << setprecision(4) << neu[0] << " +- "
445 << setprecision(4) << sqrt(QQneu[0][0])
446
447 << " dE = " << setprecision(4) << neu[1] << " +- "
448 << setprecision(4) << sqrt(QQneu[1][1])
449
450 << " dU = " << setprecision(4) << neu[2] << " +- "
451 << setprecision(4) << sqrt(QQneu[2][2]);
452 if (ambFixRate > 0.0) {
453 LOG << " fix ";
454 }
455 else {
456 LOG << " flt ";
457 }
458 LOG << int(100*ambFixRate) << " %\n";
459 }
460}
461
Note: See TracBrowser for help on using the repository browser.