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

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