source: ntrip/trunk/BNC/src/PPP/pppSatObs.cpp@ 6028

Last change on this file since 6028 was 6028, checked in by mervart, 10 years ago
File size: 15.6 KB
Line 
1// Part of BNC, a utility for retrieving decoding and
2// converting GNSS data streams from NTRIP broadcasters.
3//
4// Copyright (C) 2007
5// German Federal Agency for Cartography and Geodesy (BKG)
6// http://www.bkg.bund.de
7// Czech Technical University Prague, Department of Geodesy
8// http://www.fsv.cvut.cz
9//
10// Email: euref-ip@bkg.bund.de
11//
12// This program is free software; you can redistribute it and/or
13// modify it under the terms of the GNU General Public License
14// as published by the Free Software Foundation, version 2.
15//
16// This program is distributed in the hope that it will be useful,
17// but WITHOUT ANY WARRANTY; without even the implied warranty of
18// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19// GNU General Public License for more details.
20//
21// You should have received a copy of the GNU General Public License
22// along with this program; if not, write to the Free Software
23// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
24
25/* -------------------------------------------------------------------------
26 * BKG NTRIP Client
27 * -------------------------------------------------------------------------
28 *
29 * Class: t_pppSatObs
30 *
31 * Purpose: Satellite observations
32 *
33 * Author: L. Mervart
34 *
35 * Created: 29-Jul-2014
36 *
37 * Changes:
38 *
39 * -----------------------------------------------------------------------*/
40
41
42#include <iostream>
43#include <cmath>
44#include <newmatio.h>
45
46#include "pppSatObs.h"
47#include "bncconst.h"
48#include "pppEphPool.h"
49#include "pppStation.h"
50#include "bncutils.h"
51#include "bncantex.h"
52#include "pppObsPool.h"
53#include "pppClient.h"
54
55using namespace BNC_PPP;
56using namespace std;
57
58// Constructor
59////////////////////////////////////////////////////////////////////////////
60t_pppSatObs::t_pppSatObs(const t_satObs& pppSatObs) {
61 _prn = pppSatObs._prn;
62 _time = pppSatObs._time;
63 _outlier = false;
64 _valid = true;
65 for (unsigned ii = 0; ii < t_frequency::max; ii++) {
66 _obs[ii] = 0;
67 }
68 prepareObs(pppSatObs);
69}
70
71// Destructor
72////////////////////////////////////////////////////////////////////////////
73t_pppSatObs::~t_pppSatObs() {
74 for (unsigned iFreq = 1; iFreq < t_frequency::max; iFreq++) {
75 delete _obs[iFreq];
76 }
77}
78
79//
80////////////////////////////////////////////////////////////////////////////
81void t_pppSatObs::prepareObs(const t_satObs& pppSatObs) {
82
83 _model.reset();
84
85 // Select pseudoranges and phase observations
86 // ------------------------------------------
87 const string preferredAttrib = "CWP_";
88
89 for (unsigned iFreq = 1; iFreq < t_frequency::max; iFreq++) {
90 string frqNum = t_frequency::toString(t_frequency::type(iFreq)).substr(1);
91 for (unsigned iPref = 0; iPref < preferredAttrib.length(); iPref++) {
92 string obsType = (preferredAttrib[iPref] == '_') ? frqNum : frqNum + preferredAttrib[iPref];
93 if (_obs[iFreq] == 0) {
94 for (unsigned ii = 0; ii < pppSatObs._obs.size(); ii++) {
95 const t_frqObs* obs = pppSatObs._obs[ii];
96 if (obs->_rnxType2ch == obsType && obs->_codeValid && obs->_phaseValid) {
97 _obs[iFreq] = new t_frqObs(*obs);
98 _obs[iFreq]->_freqType = t_frequency::type(iFreq);
99 }
100 }
101 }
102 }
103 }
104
105 // Used frequency types
106 // --------------------
107 _fType1 = t_lc::toFreq(_prn.system(),t_lc::l1);
108 _fType2 = t_lc::toFreq(_prn.system(),t_lc::l2);
109
110 // Check whether all required frequencies available
111 // ------------------------------------------------
112 for (unsigned ii = 0; ii < OPT->LCs(_prn.system()).size(); ii++) {
113 t_lc::type tLC = OPT->LCs(_prn.system())[ii];
114 if (!isValid(tLC)) {
115 _valid = false;
116 return;
117 }
118 }
119
120 // Find Glonass Channel Number
121 // ---------------------------
122 if (_prn.system() == 'R') {
123 _channel = PPP_CLIENT->ephPool()->getChannel(_prn);
124 }
125 else {
126 _channel = 0;
127 }
128
129 // Compute Satellite Coordinates at Time of Transmission
130 // -----------------------------------------------------
131 _xcSat.ReSize(4); _xcSat = 0.0;
132 _vvSat.ReSize(4); _vvSat = 0.0;
133 bool totOK = false;
134 ColumnVector satPosOld(4); satPosOld = 0.0;
135 t_lc::type tLC = isValid(t_lc::cIF) ? t_lc::cIF : t_lc::c1;
136 double prange = obsValue(tLC);
137 for (int ii = 1; ii <= 10; ii++) {
138 bncTime ToT = _time - prange / t_CST::c - _xcSat[3];
139 if (PPP_CLIENT->ephPool()->getCrd(_prn, ToT, _xcSat, _vvSat) != success) {
140 _valid = false;
141 return;
142 }
143 ColumnVector dx = _xcSat - satPosOld;
144 dx[3] *= t_CST::c;
145 if (dx.norm_Frobenius() < 1.e-4) {
146 totOK = true;
147 break;
148 }
149 satPosOld = _xcSat;
150 }
151 if (totOK) {
152 _model._satClkM = _xcSat[3] * t_CST::c;
153 }
154 else {
155 _valid = false;
156 }
157}
158
159//
160////////////////////////////////////////////////////////////////////////////
161void t_pppSatObs::lcCoeff(t_lc::type tLC,
162 map<t_frequency::type, double>& codeCoeff,
163 map<t_frequency::type, double>& phaseCoeff) const {
164
165 codeCoeff.clear();
166 phaseCoeff.clear();
167
168 double f1 = t_CST::freq(_fType1, _channel);
169 double f2 = t_CST::freq(_fType2, _channel);
170
171 switch (tLC) {
172 case t_lc::l1:
173 phaseCoeff[_fType1] = 1.0;
174 return;
175 case t_lc::l2:
176 phaseCoeff[_fType2] = 1.0;
177 return;
178 case t_lc::lIF:
179 phaseCoeff[_fType1] = f1 * f1 / (f1 * f1 - f2 * f2);
180 phaseCoeff[_fType2] = -f2 * f2 / (f1 * f1 - f2 * f2);
181 return;
182 case t_lc::MW:
183 phaseCoeff[_fType1] = f1 / (f1 - f2);
184 phaseCoeff[_fType2] = -f2 / (f1 - f2);
185 codeCoeff[_fType1] = -f1 / (f1 + f2);
186 codeCoeff[_fType2] = -f2 / (f1 + f2);
187 return;
188 case t_lc::CL:
189 phaseCoeff[_fType1] = 0.5;
190 codeCoeff[_fType1] = 0.5;
191 return;
192 case t_lc::c1:
193 codeCoeff[_fType1] = 1.0;
194 return;
195 case t_lc::c2:
196 codeCoeff[_fType2] = 1.0;
197 return;
198 case t_lc::cIF:
199 codeCoeff[_fType1] = f1 * f1 / (f1 * f1 - f2 * f2);
200 codeCoeff[_fType2] = -f2 * f2 / (f1 * f1 - f2 * f2);
201 return;
202 case t_lc::dummy:
203 case t_lc::maxLc:
204 return;
205 }
206}
207
208//
209////////////////////////////////////////////////////////////////////////////
210bool t_pppSatObs::isValid(t_lc::type tLC) const {
211 bool valid = true;
212 obsValue(tLC, &valid);
213 return valid;
214}
215//
216////////////////////////////////////////////////////////////////////////////
217double t_pppSatObs::obsValue(t_lc::type tLC, bool* valid) const {
218
219 map<t_frequency::type, double> codeCoeff;
220 map<t_frequency::type, double> phaseCoeff;
221 lcCoeff(tLC, codeCoeff, phaseCoeff);
222
223 double retVal = 0.0;
224 if (valid) *valid = true;
225
226 map<t_frequency::type, double>::const_iterator it;
227 for (it = codeCoeff.begin(); it != codeCoeff.end(); it++) {
228 t_frequency::type tFreq = it->first;
229 if (_obs[tFreq] == 0) {
230 if (valid) *valid = false;
231 return 0.0;
232 }
233 else {
234 retVal += it->second * _obs[tFreq]->_code;
235 }
236 }
237 for (it = phaseCoeff.begin(); it != phaseCoeff.end(); it++) {
238 t_frequency::type tFreq = it->first;
239 if (_obs[tFreq] == 0) {
240 if (valid) *valid = false;
241 return 0.0;
242 }
243 else {
244 retVal += it->second * _obs[tFreq]->_phase * t_CST::lambda(tFreq, _channel);
245 }
246 }
247
248 return retVal;
249}
250
251
252//
253////////////////////////////////////////////////////////////////////////////
254t_irc t_pppSatObs::cmpModel(const t_pppStation* station) {
255
256 // Reset all model values
257 // ----------------------
258 _model.reset();
259
260 // Topocentric Satellite Position
261 // ------------------------------
262 ColumnVector rSat = _xcSat.Rows(1,3);
263 ColumnVector rhoV = rSat - station->xyzApr();
264 _model._rho = rhoV.norm_Frobenius();
265
266 ColumnVector neu(3);
267 xyz2neu(station->ellApr().data(), rhoV.data(), neu.data());
268
269 _model._eleSat = acos( sqrt(neu[0]*neu[0] + neu[1]*neu[1]) / _model._rho );
270 if (neu[2] < 0) {
271 _model._eleSat *= -1.0;
272 }
273 _model._azSat = atan2(neu[1], neu[0]);
274
275 // Satellite Clocks
276 // ----------------
277 _model._satClkM = _xcSat[3] * t_CST::c;
278
279 // Receiver Clocks
280 // ---------------
281 _model._recClkM = station->dClk() * t_CST::c;
282
283 // Sagnac Effect (correction due to Earth rotation)
284 // ------------------------------------------------
285 ColumnVector Omega(3);
286 Omega[0] = 0.0;
287 Omega[1] = 0.0;
288 Omega[2] = t_CST::omega / t_CST::c;
289 _model._sagnac = DotProduct(Omega, crossproduct(rSat, station->xyzApr()));
290
291 // Antenna Eccentricity
292 // --------------------
293 _model._antEcc = -DotProduct(station->xyzEcc(), rhoV) / _model._rho;
294
295 // Antenna Phase Center Offsets and Variations
296 // -------------------------------------------
297 if (PPP_CLIENT->antex()) {
298 bool found;
299 _model._antPco1 = PPP_CLIENT->antex()->rcvCorr(station->antName(), _model._eleSat, found);
300 _model._antPco2 = _model._antPco1;
301 }
302
303 // Tropospheric Delay
304 // ------------------
305 _model._tropo = t_tropo::delay_saast(station->xyzApr(), _model._eleSat);
306
307 // Phase Wind-Up
308 // -------------
309 _model._windUp = station->windUp(_time, _prn, rSat);
310
311 // Code (and Phase) Biases
312 // -----------------------
313 const t_satBias* satBias = PPP_CLIENT->obsPool()->satBias(_prn);
314 if (satBias) {
315 for (unsigned ii = 0; ii < satBias->_bias.size(); ii++) {
316 const t_frqBias& bias = satBias->_bias[ii];
317 if (_validObs1 && _validObs1->_rnxType2ch == bias._rnxType2ch) {
318 _validObs1->_biasJumpCounter = satBias->_jumpCount;
319 if (bias._codeValid) {
320 _model._biasC1 = bias._code;
321 }
322 if (bias._phaseValid) {
323 _model._biasL1 = bias._phase;
324 }
325 }
326 if (_validObs2 && _validObs2->_rnxType2ch == bias._rnxType2ch) {
327 _validObs2->_biasJumpCounter = satBias->_jumpCount;
328 if (bias._codeValid) {
329 _model._biasC2 = bias._code;
330 }
331 if (bias._phaseValid) {
332 _model._biasL2 = bias._phase;
333 }
334 }
335 }
336 }
337
338 // Tidal Correction
339 // ----------------
340 _model._tide = -DotProduct(station->tideDspl(), rhoV) / _model._rho;
341
342 // Ionospheric Delay
343 // -----------------
344 // TODO
345
346 // Ocean Loading
347 // -------------
348 // TODO
349
350 // Set Model Set Flag
351 // ------------------
352 _model._set = true;
353
354 return success;
355}
356
357//
358////////////////////////////////////////////////////////////////////////////
359void t_pppSatObs::printModel() const {
360 LOG.setf(ios::fixed);
361 LOG << "MODEL for Satellite " << _prn.toString() << endl
362 << "RHO: " << setw(12) << setprecision(3) << _model._rho << endl
363 << "ELE: " << setw(12) << setprecision(3) << _model._eleSat * 180.0 / M_PI << endl
364 << "AZI: " << setw(12) << setprecision(3) << _model._azSat * 180.0 / M_PI << endl
365 << "SATCLK: " << setw(12) << setprecision(3) << _model._satClkM << endl
366 << "RECCLK: " << setw(12) << setprecision(3) << _model._recClkM << endl
367 << "SAGNAC: " << setw(12) << setprecision(3) << _model._sagnac << endl
368 << "ANTECC: " << setw(12) << setprecision(3) << _model._antEcc << endl
369 << "PCO1: " << setw(12) << setprecision(3) << _model._antPco1 << endl
370 << "PCO2: " << setw(12) << setprecision(3) << _model._antPco2 << endl
371 << "TROPO: " << setw(12) << setprecision(3) << _model._tropo << endl
372 << "WINDUP: " << setw(12) << setprecision(3) << _model._windUp << endl
373 << "BIASC1: " << setw(12) << setprecision(3) << _model._biasC1 << endl
374 << "BIASC2: " << setw(12) << setprecision(3) << _model._biasC2 << endl
375 << "BIASL1: " << setw(12) << setprecision(3) << _model._biasL1 << endl
376 << "BIASL2: " << setw(12) << setprecision(3) << _model._biasL2 << endl
377 << "TIDES: " << setw(12) << setprecision(3) << _model._tide << endl;
378
379 //// beg test
380 LOG << "PCO L3: " << setw(12) << setprecision(3)
381 << lc(t_lc::lIF, _model._antPco1, _model._antPco2, 0.0, 0.0) << endl;
382
383 LOG << "WIND L3:" << setw(12) << setprecision(3)
384 << lc(t_lc::lIF, _model._windUp * t_CST::c / _f1,
385 _model._windUp * t_CST::c / _f2, 0.0, 0.0) << endl;
386
387 LOG << "OBS-CMP P3: " << _prn.toString() << " "
388 << setw(12) << setprecision(3) << obsValue(t_lc::cIF) << " "
389 << setw(12) << setprecision(3) << cmpValue(t_lc::cIF) << " "
390 << setw(12) << setprecision(3) << obsValue(t_lc::cIF) - cmpValue(t_lc::cIF) << endl;
391
392 LOG << "OBS-CMP L3: " << _prn.toString() << " "
393 << setw(12) << setprecision(3) << obsValue(t_lc::lIF) << " "
394 << setw(12) << setprecision(3) << cmpValue(t_lc::lIF) << " "
395 << setw(12) << setprecision(3) << obsValue(t_lc::lIF) - cmpValue(t_lc::lIF) << endl;
396
397 LOG << "OBS-CMP MW: " << _prn.toString() << " "
398 << setw(12) << setprecision(3) << obsValue(t_lc::MW) << " "
399 << setw(12) << setprecision(3) << cmpValue(t_lc::MW) << " "
400 << setw(12) << setprecision(3) << obsValue(t_lc::MW) - cmpValue(t_lc::MW) << endl;
401 //// end test
402}
403
404//
405////////////////////////////////////////////////////////////////////////////
406double t_pppSatObs::cmpValueForBanc(t_lc::type tLC) const {
407 return cmpValue(tLC) - _model._rho - _model._sagnac - _model._recClkM;
408}
409
410//
411////////////////////////////////////////////////////////////////////////////
412double t_pppSatObs::cmpValue(t_lc::type tLC) const {
413
414 if (!_validObs2 && t_lc::need2ndFreq(tLC)) {
415 return 0.0;
416 }
417
418 // Non-Dispersive Part
419 // -------------------
420 double nonDisp = _model._rho + _model._recClkM - _model._satClkM
421 + _model._sagnac + _model._antEcc + _model._tropo
422 + _model._tide;
423
424 // Add Dispersive Part
425 // -------------------
426 double L1 = nonDisp + _model._antPco1 - _model._biasL1 + _model._windUp * t_CST::c / _f1;
427 double L2 = nonDisp + _model._antPco2 - _model._biasL2 + _model._windUp * t_CST::c / _f2;
428 double C1 = nonDisp + _model._antPco1 - _model._biasC1;
429 double C2 = nonDisp + _model._antPco2 - _model._biasC2;
430
431 return this->lc(tLC, L1, L2, C1, C2);
432}
433
434//
435////////////////////////////////////////////////////////////////////////////
436double t_pppSatObs::lambda(t_lc::type tLC) const {
437
438 if (tLC == t_lc::l1) {
439 return t_CST::c / _f1;
440 }
441 else if (tLC == t_lc::l2) {
442 return t_CST::c / _f2;
443 }
444 else if (tLC == t_lc::lIF) {
445 return t_CST::c / (_f1 + _f2);
446 }
447 else if (tLC == t_lc::MW) {
448 return t_CST::c / (_f1 - _f2);
449 }
450 else if (tLC == t_lc::CL) {
451 return t_CST::c / _f1 / 2.0;
452 }
453
454 return 0.0;
455}
456
457//
458////////////////////////////////////////////////////////////////////////////
459double t_pppSatObs::sigma(t_lc::type tLC) const {
460
461 ColumnVector sig(4);
462 sig(1) = OPT->_sigmaL1;
463 sig(2) = OPT->_sigmaL1;
464 sig(3) = OPT->_sigmaC1;
465 sig(4) = OPT->_sigmaC1;
466
467 ColumnVector coeff(4);
468 lc(tLC, sig(1), sig(2), sig(3), sig(4), &coeff);
469
470 ColumnVector sp = SP(sig, coeff); // Schur product
471
472 // Elevation-Dependent Weighting
473 // -----------------------------
474 double cEle = 1.0;
475 if ( (OPT->_eleWgtCode && t_lc::includesCode(tLC)) ||
476 (OPT->_eleWgtPhase && t_lc::includesPhase(tLC)) ) {
477 double eleD = eleSat()*180.0/M_PI;
478 double hlp = fabs(90.0 - eleD);
479 cEle = (1.0 + hlp*hlp*hlp*0.000004);
480 }
481
482 return cEle * sp.norm_Frobenius();
483}
484
485//
486////////////////////////////////////////////////////////////////////////////
487double t_pppSatObs::maxRes(t_lc::type tLC) const {
488
489 ColumnVector res(4);
490 res(1) = OPT->_maxResL1;
491 res(2) = OPT->_maxResL1;
492 res(3) = OPT->_maxResC1;
493 res(4) = OPT->_maxResC1;
494
495 ColumnVector coeff(4);
496 lc(tLC, res(1), res(2), res(3), res(4), &coeff);
497
498 ColumnVector sp = SP(res, coeff); // Schur product
499
500 return sp.norm_Frobenius();
501}
502
503//
504////////////////////////////////////////////////////////////////////////////
505void t_pppSatObs::setRes(t_lc::type tLC, double res) {
506 _res[tLC] = res;
507}
508
509//
510////////////////////////////////////////////////////////////////////////////
511double t_pppSatObs::getRes(t_lc::type tLC) const {
512 map<t_lc::type, double>::const_iterator it = _res.find(tLC);
513 if (it != _res.end()) {
514 return it->second;
515 }
516 else {
517 return 0.0;
518 }
519}
Note: See TracBrowser for help on using the repository browser.