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

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