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

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