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

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