source: ntrip/trunk/BNC/src/PPP/satobs.cpp@ 5744

Last change on this file since 5744 was 5743, checked in by mervart, 10 years ago
File size: 17.4 KB
RevLine 
[5743]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_satObs
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#include "satobs.h"
46#include "genconst.h"
47#include "ephpool.h"
48#include "station.h"
49#include "utils.h"
50#include "tropo.h"
51#include "antex.h"
52#include "obspool.h"
53
54using namespace BNC;
55using namespace std;
56
57// Constructor
58////////////////////////////////////////////////////////////////////////////
59t_satObs::t_obs::t_obs(const t_obs& obs) {
60 _type = obs._rnxType2ch;
61 _code = obs._code;
62 _codeValid = obs._codeValid;
63 _phase = obs._phase;
64 _phaseValid = obs._phaseValid;
65 _doppler = obs._doppler;
66 _dopplerValid = obs._dopplerValid;
67 _snr = obs._snr;
68 _snrValid = obs._snrValid;
69 _slip = obs._slip;
70 _slipCounter = obs._slipCounter;
71 _biasJumpCounter = -1;
72}
73
74// Constructor
75////////////////////////////////////////////////////////////////////////////
76t_satObs::t_satObs(const t_satObs& satObs) {
77 _prn = satObs._prn;
78 _time = satObs._time;
79 _outlier = false;
80 for (int ii = 0; ii < satObs._numObs; ii++) {
81 const t_obs& obs = satObs._obs[ii];
82 t_obsType obsType = string(obs._rnxType2ch).substr(0,2);
83 _allObs[obsType] = new t_obs(obs);
84 }
85 prepareObs();
86}
87
88// Destructor
89////////////////////////////////////////////////////////////////////////////
90t_satObs::~t_satObs() {
91 map<t_obsType, t_obs*>::const_iterator it;
92 for (it = _allObs.begin(); it != _allObs.end(); it++) {
93 delete it->second;
94 }
95}
96
97//
98////////////////////////////////////////////////////////////////////////////
99void t_satObs::prepareObs() {
100 _model.reset();
101 _valid = true;
102 _validObs1 = 0;
103 _validObs2 = 0;
104
105 bool dualFreq = OPT->dualFreqRequired();
106
107 // Select two pseudoranges and two phase observations
108 // --------------------------------------------------
109 const string preferredAttrib = "CWP";
110 for (unsigned iPref = 0; iPref < preferredAttrib.length(); iPref++) {
111 t_obsType obsType1 = "1?";
112 obsType1[1] = preferredAttrib[iPref];
113 if (_validObs1 == 0 && _allObs.find(obsType1) != _allObs.end()) {
114 t_obs* obs = _allObs[obsType1];
115 if (obs->_codeValid && obs->_phaseValid) {
116 _validObs1 = obs;
117 }
118 }
119 if (dualFreq) {
120 t_obsType obsType2 = "2?";
121 obsType2[1] = preferredAttrib[iPref];
122 if (_validObs2 == 0 && _allObs.find(obsType2) != _allObs.end()) {
123 t_obs* obs = _allObs[obsType2];
124 if (obs->_codeValid && obs->_phaseValid) {
125 _validObs2 = obs;
126 }
127 }
128 }
129 }
130
131 if (_validObs1 == 0 || (dualFreq && _validObs2 == 0)) {
132 _valid = false;
133 return;
134 }
135
136 // Find Glonass Channel Number
137 // ---------------------------
138 if (_prn.system() == 'R') {
139 _channel = PPP_CLIENT->ephPool()->getChannel(_prn);
140 }
141 else {
142 _channel = 0;
143 }
144
145 // Copy raw observations
146 // ---------------------
147 _f1 = t_genConst::f1(_prn.system(), _channel);
148 _rawC1 = _validObs1->_code;
149 _rawL1 = _validObs1->_phase * t_genConst::c / _f1;
150 if (dualFreq) {
151 _f2 = t_genConst::f2(_prn.system(), _channel);
152 _rawC2 = _validObs2->_code;
153 _rawL2 = _validObs2->_phase * t_genConst::c / _f2;
154 }
155 else {
156 _f2 = 0.0;
157 _rawC2 = 0.0;
158 _rawL2 = 0.0;
159 }
160
161 // Compute Satellite Coordinates at Time of Transmission
162 // -----------------------------------------------------
163 _xcSat.ReSize(4); _xcSat = 0.0;
164 _vvSat.ReSize(4); _vvSat = 0.0;
165 bool totOK = false;
166 ColumnVector satPosOld(4); satPosOld = 0.0;
167 t_lc::type tLC = (dualFreq ? t_lc::cIF : t_lc::c1);
168 double prange = obsValue(tLC);
169 for (int ii = 1; ii <= 10; ii++) {
170 bncTime ToT = _time - prange / t_genConst::c - _xcSat[3];
171 if (PPP_CLIENT->ephPool()->getCrd(_prn, ToT, _xcSat, _vvSat) != t_irc::success) {
172 _valid = false;
173 return;
174 }
175 ColumnVector dx = _xcSat - satPosOld;
176 dx[3] *= t_genConst::c;
177 if (dx.norm_Frobenius() < 1.e-4) {
178 totOK = true;
179 break;
180 }
181 satPosOld = _xcSat;
182 }
183 if (totOK) {
184 _model._satClkM = _xcSat[3] * t_genConst::c;
185 }
186 else {
187 _valid = false;
188 }
189}
190
191//
192////////////////////////////////////////////////////////////////////////////
193t_irc t_satObs::cmpModel(const t_station* station) {
194
195 // Reset all model values
196 // ----------------------
197 _model.reset();
198
199 // Topocentric Satellite Position
200 // ------------------------------
201 ColumnVector rSat = _xcSat.Rows(1,3);
202 ColumnVector rhoV = rSat - station->xyzApr();
203 _model._rho = rhoV.norm_Frobenius();
204
205 ColumnVector neu(3);
206 xyz2neu(station->ellApr().data(), rhoV.data(), neu.data());
207
208 _model._eleSat = acos( sqrt(neu[0]*neu[0] + neu[1]*neu[1]) / _model._rho );
209 if (neu[2] < 0) {
210 _model._eleSat *= -1.0;
211 }
212 _model._azSat = atan2(neu[1], neu[0]);
213
214 // Satellite Clocks
215 // ----------------
216 _model._satClkM = _xcSat[3] * t_genConst::c;
217
218 // Receiver Clocks
219 // ---------------
220 _model._recClkM = station->dClk() * t_genConst::c;
221
222 // Sagnac Effect (correction due to Earth rotation)
223 // ------------------------------------------------
224 ColumnVector Omega(3);
225 Omega[0] = 0.0;
226 Omega[1] = 0.0;
227 Omega[2] = t_genConst::omega / t_genConst::c;
228 _model._sagnac = DotProduct(Omega, crossproduct(rSat, station->xyzApr()));
229
230 // Antenna Eccentricity
231 // --------------------
232 _model._antEcc = -DotProduct(station->xyzEcc(), rhoV) / _model._rho;
233
234 // Antenna Phase Center Offsets and Variations
235 // -------------------------------------------
236 if (PPP_CLIENT->antex()) {
237 t_frequency::type frq1 = t_frequency::G1;
238 t_frequency::type frq2 = t_frequency::G2;
239 if (_prn.system() == 'R') {
240 frq1 = t_frequency::R1;
241 frq2 = t_frequency::R2;
242 }
243 bool found;
244 _model._antPco1 = PPP_CLIENT->antex()->rcvCorr(frq1, station->antName(),
245 _model._eleSat, found);
246 _model._antPco2 = PPP_CLIENT->antex()->rcvCorr(frq2, station->antName(),
247 _model._eleSat, found);
248 }
249
250 // Tropospheric Delay
251 // ------------------
252 t_tropo::dtrop(_time, station->ellApr()[0], station->ellApr()[1],
253 station->ellApr()[2], _model._eleSat, OPT->tropoModel(),
254 OPT->tropoMF(), false, _model._tropo);
255
256 // Phase Wind-Up
257 // -------------
258 _model._windUp = station->windUp(_time, _prn, rSat);
259
260 // Code (and Phase) Biases
261 // -----------------------
262 bool biasC1flg = false;
263 bool biasC2flg = false;
264 bool biasL1flg = false;
265 bool biasL2flg = false;
266 int nsdfix = 0;
267 const t_satBias* satBias = PPP_CLIENT->obsPool()->satBias(_prn);
268 if (satBias) {
269 nsdfix = satBias->nx();
270 map<t_biasType, double>::const_iterator it;
271 for (it = satBias->biases().begin(); it != satBias->biases().end(); it++) {
272 const t_biasType& biasType = it->first;
273 if (_validObs1) {
274 _validObs1->_biasJumpCounter = satBias->jumpCount();
275 if ("C" + _validObs1->_type == biasType) {
276 _model._biasC1 = it->second;
277 biasC1flg = true;
278 }
279 else if ("L" + _validObs1->_type == biasType) {
280 _model._biasL1 = it->second;
281 biasL1flg = true;
282 }
283 }
284 if (_validObs2) {
285 _validObs2->_biasJumpCounter = satBias->jumpCount();
286 if ("C" + _validObs2->_type == biasType) {
287 _model._biasC2 = it->second;
288 biasC2flg = true;
289 }
290 else if ("L" + _validObs2->_type == biasType) {
291 _model._biasL2 = it->second;
292 biasL2flg = true;
293 }
294 }
295 }
296 }
297 if (_prn.system() == 'G' && OPT->biasRequired()) {
298 if (!biasC1flg || !biasC2flg || !biasL1flg || !biasL2flg) {
299 _valid = false;
300 }
301 if (nsdfix < OPT->minSDFix()) {
302 _valid = false;
303 }
304 }
305
306 // Tidal Correction
307 // ----------------
308 _model._tide = -DotProduct(station->tideDspl(), rhoV) / _model._rho;
309
310 // Ionospheric Delay
311 // -----------------
312 // TODO
313
314 // Ocean Loading
315 // -------------
316 // TODO
317
318 // Set Model Set Flag
319 // ------------------
320 _model._set = true;
321
322 return t_irc::success;
323}
324
325//
326////////////////////////////////////////////////////////////////////////////
327void t_satObs::printModel() const {
328 LOG.setf(ios::fixed);
329 LOG << "MODEL for Satellite " << _prn.toString() << endl
330 << "RHO: " << setw(12) << setprecision(3) << _model._rho << endl
331 << "ELE: " << setw(12) << setprecision(3) << _model._eleSat * t_genConst::rho_deg << endl
332 << "AZI: " << setw(12) << setprecision(3) << _model._azSat * t_genConst::rho_deg << endl
333 << "SATCLK: " << setw(12) << setprecision(3) << _model._satClkM << endl
334 << "RECCLK: " << setw(12) << setprecision(3) << _model._recClkM << endl
335 << "SAGNAC: " << setw(12) << setprecision(3) << _model._sagnac << endl
336 << "ANTECC: " << setw(12) << setprecision(3) << _model._antEcc << endl
337 << "PCO1: " << setw(12) << setprecision(3) << _model._antPco1 << endl
338 << "PCO2: " << setw(12) << setprecision(3) << _model._antPco2 << endl
339 << "TROPO: " << setw(12) << setprecision(3) << _model._tropo << endl
340 << "WINDUP: " << setw(12) << setprecision(3) << _model._windUp << endl
341 << "BIASC1: " << setw(12) << setprecision(3) << _model._biasC1 << endl
342 << "BIASC2: " << setw(12) << setprecision(3) << _model._biasC2 << endl
343 << "BIASL1: " << setw(12) << setprecision(3) << _model._biasL1 << endl
344 << "BIASL2: " << setw(12) << setprecision(3) << _model._biasL2 << endl
345 << "TIDES: " << setw(12) << setprecision(3) << _model._tide << endl;
346
347 //// beg test
348 LOG << "PCO L3: " << setw(12) << setprecision(3)
349 << lc(t_lc::lIF, _model._antPco1, _model._antPco2, 0.0, 0.0) << endl;
350
351 LOG << "WIND L3:" << setw(12) << setprecision(3)
352 << lc(t_lc::lIF, _model._windUp * t_genConst::c / _f1,
353 _model._windUp * t_genConst::c / _f2, 0.0, 0.0) << endl;
354
355 LOG << "OBS-CMP P3: " << _prn.toString() << " "
356 << setw(12) << setprecision(3) << obsValue(t_lc::cIF) << " "
357 << setw(12) << setprecision(3) << cmpValue(t_lc::cIF) << " "
358 << setw(12) << setprecision(3) << obsValue(t_lc::cIF) - cmpValue(t_lc::cIF) << endl;
359
360 LOG << "OBS-CMP L3: " << _prn.toString() << " "
361 << setw(12) << setprecision(3) << obsValue(t_lc::lIF) << " "
362 << setw(12) << setprecision(3) << cmpValue(t_lc::lIF) << " "
363 << setw(12) << setprecision(3) << obsValue(t_lc::lIF) - cmpValue(t_lc::lIF) << endl;
364
365 LOG << "OBS-CMP MW: " << _prn.toString() << " "
366 << setw(12) << setprecision(3) << obsValue(t_lc::MW) << " "
367 << setw(12) << setprecision(3) << cmpValue(t_lc::MW) << " "
368 << setw(12) << setprecision(3) << obsValue(t_lc::MW) - cmpValue(t_lc::MW) << endl;
369 //// end test
370}
371
372//
373////////////////////////////////////////////////////////////////////////////
374double t_satObs::obsValue(t_lc::type tLC) const {
375
376 if (!_validObs2 && t_lc::need2ndFreq(tLC)) {
377 return 0.0;
378 }
379
380 return this->lc(tLC, _rawL1, _rawL2, _rawC1, _rawC2);
381}
382
383//
384////////////////////////////////////////////////////////////////////////////
385double t_satObs::cmpValueForBanc(t_lc::type tLC) const {
386 return cmpValue(tLC) - _model._rho - _model._sagnac - _model._recClkM;
387}
388
389//
390////////////////////////////////////////////////////////////////////////////
391double t_satObs::cmpValue(t_lc::type tLC) const {
392
393 if (!_validObs2 && t_lc::need2ndFreq(tLC)) {
394 return 0.0;
395 }
396
397 // Non-Dispersive Part
398 // -------------------
399 double nonDisp = _model._rho + _model._recClkM - _model._satClkM
400 + _model._sagnac + _model._antEcc + _model._tropo
401 + _model._tide;
402
403 // Add Dispersive Part
404 // -------------------
405 double L1 = nonDisp + _model._antPco1 - _model._biasL1 + _model._windUp * t_genConst::c / _f1;
406 double L2 = nonDisp + _model._antPco2 - _model._biasL2 + _model._windUp * t_genConst::c / _f2;
407 double C1 = nonDisp + _model._antPco1 - _model._biasC1;
408 double C2 = nonDisp + _model._antPco2 - _model._biasC2;
409
410 return this->lc(tLC, L1, L2, C1, C2);
411}
412
413//
414////////////////////////////////////////////////////////////////////////////
415double t_satObs::lc(t_lc::type tLC,
416 double L1, double L2, double C1, double C2,
417 ColumnVector* coeff) const {
418
419 if (coeff) {
420 coeff->ReSize(4);
421 (*coeff) = 0.0;
422 }
423
424 if (tLC == t_lc::l1) {
425 if (coeff) (*coeff)(1) = 1.0;
426 return L1;
427 }
428 else if (tLC == t_lc::l2) {
429 if (coeff) (*coeff)(2) = 1.0;
430 return L2;
431 }
432 else if (tLC == t_lc::c1) {
433 if (coeff) (*coeff)(3) = 1.0;
434 return C1;
435 }
436 else if (tLC == t_lc::c2) {
437 if (coeff) (*coeff)(4) = 1.0;
438 return C2;
439 }
440 else if (tLC == t_lc::lIF || tLC == t_lc::cIF) {
441 double a1 = _f1 * _f1 / (_f1 * _f1 - _f2 * _f2);
442 double a2 = -_f2 * _f2 / (_f1 * _f1 - _f2 * _f2);
443 if (tLC == t_lc::lIF) {
444 if (coeff) {
445 (*coeff)(1) = a1;
446 (*coeff)(2) = a2;
447 }
448 return a1 * L1 + a2 * L2;
449 }
450 else {
451 if (coeff) {
452 (*coeff)(3) = a1;
453 (*coeff)(4) = a2;
454 }
455 return a1 * C1 + a2 * C2;
456 }
457 }
458 else if (tLC == t_lc::MW) {
459 double a1 = _f1 / (_f1 - _f2);
460 double a2 = -_f2 / (_f1 - _f2);
461 double a3 = -_f1 / (_f1 + _f2);
462 double a4 = -_f2 / (_f1 + _f2);
463 if (coeff) {
464 (*coeff)(1) = a1;
465 (*coeff)(2) = a2;
466 (*coeff)(3) = a3;
467 (*coeff)(4) = a4;
468 }
469 return a1 * L1 + a2 * L2 + a3 * C1 + a4 * C2;
470 }
471 else if (tLC == t_lc::CL) {
472 if (coeff) {
473 (*coeff)(1) = 0.5;
474 (*coeff)(3) = 0.5;
475 }
476 return (C1 + L1) / 2.0;
477 }
478
479 return 0.0;
480}
481
482//
483////////////////////////////////////////////////////////////////////////////
484t_irc t_satObs::createDifference(const t_satObs* obsBase) {
485
486 _rawC1 -= obsBase->_rawC1;
487 _rawC2 -= obsBase->_rawC2;
488 _rawL1 -= obsBase->_rawL1;
489 _rawL2 -= obsBase->_rawL2;
490 _model._rho -= obsBase->_model._rho;
491 _model._recClkM -= obsBase->_model._recClkM;
492 _model._satClkM -= obsBase->_model._satClkM;
493 _model._sagnac -= obsBase->_model._sagnac;
494 _model._antEcc -= obsBase->_model._antEcc;
495 _model._tropo -= obsBase->_model._tropo;
496 _model._tide -= obsBase->_model._tide;
497 _model._windUp -= obsBase->_model._windUp;
498 _model._antPco1 -= obsBase->_model._antPco1;
499 _model._antPco2 -= obsBase->_model._antPco2;
500 _model._biasC1 -= obsBase->_model._biasC1;
501 _model._biasC2 -= obsBase->_model._biasC2;
502 _model._biasL1 -= obsBase->_model._biasL1;
503 _model._biasL2 -= obsBase->_model._biasL2;
504
505 return t_irc::success;
506}
507
508//
509////////////////////////////////////////////////////////////////////////////
510double t_satObs::lambda(t_lc::type tLC) const {
511
512 if (tLC == t_lc::l1) {
513 return t_genConst::c / _f1;
514 }
515 else if (tLC == t_lc::l2) {
516 return t_genConst::c / _f2;
517 }
518 else if (tLC == t_lc::lIF) {
519 return t_genConst::c / (_f1 + _f2);
520 }
521 else if (tLC == t_lc::MW) {
522 return t_genConst::c / (_f1 - _f2);
523 }
524 else if (tLC == t_lc::CL) {
525 return t_genConst::c / _f1 / 2.0;
526 }
527
528 return 0.0;
529}
530
531//
532////////////////////////////////////////////////////////////////////////////
533double t_satObs::sigma(t_lc::type tLC) const {
534
535 ColumnVector sig(4);
536 sig(1) = OPT->sigmaPhase();
537 sig(2) = OPT->sigmaPhase();
538 sig(3) = OPT->sigmaCode();
539 sig(4) = OPT->sigmaCode();
540
541 ColumnVector coeff(4);
542 lc(tLC, sig(1), sig(2), sig(3), sig(4), &coeff);
543
544 ColumnVector sp = SP(sig, coeff); // Schur product
545
546 // Elevation-Dependent Weighting
547 // -----------------------------
548 double cEle = 1.0;
549 if ( (OPT->eleWgtCode() && t_lc::includesCode(tLC)) ||
550 (OPT->eleWgtPhase() && t_lc::includesPhase(tLC)) ) {
551 double eleD = eleSat()*180.0/t_genConst::pi;
552 double hlp = fabs(90.0 - eleD);
553 cEle = (1.0 + hlp*hlp*hlp*0.000004);
554 }
555
556 return cEle * sp.norm_Frobenius();
557}
558
559//
560////////////////////////////////////////////////////////////////////////////
561void t_satObs::setRes(t_lc::type tLC, double res) {
562 _res[tLC] = res;
563}
564
565//
566////////////////////////////////////////////////////////////////////////////
567double t_satObs::getRes(t_lc::type tLC) const {
568 map<t_lc::type, double>::const_iterator it = _res.find(tLC);
569 if (it != _res.end()) {
570 return it->second;
571 }
572 else {
573 return 0.0;
574 }
575}
Note: See TracBrowser for help on using the repository browser.