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

Last change on this file since 6015 was 6015, checked in by mervart, 10 years ago
File size: 15.2 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 for (unsigned ii = 0; ii < pppSatObs._obs.size(); ii++) {
65 _allObs.push_back(new t_frqObs(*pppSatObs._obs[ii]));
66 }
67 prepareObs();
68}
69
70// Destructor
71////////////////////////////////////////////////////////////////////////////
72t_pppSatObs::~t_pppSatObs() {
73 for (unsigned ii = 0; ii < _allObs.size(); ii++) {
74 delete _allObs[ii];
75 }
76}
77
78//
79////////////////////////////////////////////////////////////////////////////
80void t_pppSatObs::prepareObs() {
81 _model.reset();
82 _valid = true;
83 _validObs1 = 0;
84 _validObs2 = 0;
85
86 bool dualFreq = OPT->dualFreqRequired(_prn.system());
87
88 // Select two pseudoranges and two phase observations
89 // --------------------------------------------------
90 const string preferredAttrib = "CWP_";
91 for (unsigned iPref = 0; iPref < preferredAttrib.length(); iPref++) {
92 string obsType1 = (preferredAttrib[iPref] == '_') ? string("1") : string("1") + preferredAttrib[iPref];
93 string obsType2 = (preferredAttrib[iPref] == '_') ? string("2") : string("2") + preferredAttrib[iPref];
94 if (_validObs1 == 0) {
95 for (unsigned ii = 0; ii < _allObs.size(); ii++) {
96 t_frqObs* obs = _allObs[ii];
97 if (obs->_rnxType2ch == obsType1 && obs->_codeValid && obs->_phaseValid) {
98 _validObs1 = obs;
99 }
100 }
101 }
102 if (dualFreq) {
103 if (_validObs2 == 0) {
104 for (unsigned ii = 0; ii < _allObs.size(); ii++) {
105 t_frqObs* obs = _allObs[ii];
106 if (obs->_rnxType2ch == obsType2 && obs->_codeValid && obs->_phaseValid) {
107 _validObs2 = obs;
108 }
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_pppSatObs::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 bool found;
221 _model._antPco1 = PPP_CLIENT->antex()->rcvCorr(station->antName(), _model._eleSat, found);
222 _model._antPco2 = _model._antPco1;
223 }
224
225 // Tropospheric Delay
226 // ------------------
227 _model._tropo = t_tropo::delay_saast(station->xyzApr(), _model._eleSat);
228
229 // Phase Wind-Up
230 // -------------
231 _model._windUp = station->windUp(_time, _prn, rSat);
232
233 // Code (and Phase) Biases
234 // -----------------------
235 const t_satBias* satBias = PPP_CLIENT->obsPool()->satBias(_prn);
236 if (satBias) {
237 for (unsigned ii = 0; ii < satBias->_bias.size(); ii++) {
238 const t_frqBias& bias = satBias->_bias[ii];
239 if (_validObs1 && _validObs1->_rnxType2ch == bias._rnxType2ch) {
240 _validObs1->_biasJumpCounter = satBias->_jumpCount;
241 if (bias._codeValid) {
242 _model._biasC1 = bias._code;
243 }
244 if (bias._phaseValid) {
245 _model._biasL1 = bias._phase;
246 }
247 }
248 if (_validObs2 && _validObs2->_rnxType2ch == bias._rnxType2ch) {
249 _validObs2->_biasJumpCounter = satBias->_jumpCount;
250 if (bias._codeValid) {
251 _model._biasC2 = bias._code;
252 }
253 if (bias._phaseValid) {
254 _model._biasL2 = bias._phase;
255 }
256 }
257 }
258 }
259
260 // Tidal Correction
261 // ----------------
262 _model._tide = -DotProduct(station->tideDspl(), rhoV) / _model._rho;
263
264 // Ionospheric Delay
265 // -----------------
266 // TODO
267
268 // Ocean Loading
269 // -------------
270 // TODO
271
272 // Set Model Set Flag
273 // ------------------
274 _model._set = true;
275
276 return success;
277}
278
279//
280////////////////////////////////////////////////////////////////////////////
281void t_pppSatObs::printModel() const {
282 LOG.setf(ios::fixed);
283 LOG << "MODEL for Satellite " << _prn.toString() << endl
284 << "RHO: " << setw(12) << setprecision(3) << _model._rho << endl
285 << "ELE: " << setw(12) << setprecision(3) << _model._eleSat * 180.0 / M_PI << endl
286 << "AZI: " << setw(12) << setprecision(3) << _model._azSat * 180.0 / M_PI << endl
287 << "SATCLK: " << setw(12) << setprecision(3) << _model._satClkM << endl
288 << "RECCLK: " << setw(12) << setprecision(3) << _model._recClkM << endl
289 << "SAGNAC: " << setw(12) << setprecision(3) << _model._sagnac << endl
290 << "ANTECC: " << setw(12) << setprecision(3) << _model._antEcc << endl
291 << "PCO1: " << setw(12) << setprecision(3) << _model._antPco1 << endl
292 << "PCO2: " << setw(12) << setprecision(3) << _model._antPco2 << endl
293 << "TROPO: " << setw(12) << setprecision(3) << _model._tropo << endl
294 << "WINDUP: " << setw(12) << setprecision(3) << _model._windUp << endl
295 << "BIASC1: " << setw(12) << setprecision(3) << _model._biasC1 << endl
296 << "BIASC2: " << setw(12) << setprecision(3) << _model._biasC2 << endl
297 << "BIASL1: " << setw(12) << setprecision(3) << _model._biasL1 << endl
298 << "BIASL2: " << setw(12) << setprecision(3) << _model._biasL2 << endl
299 << "TIDES: " << setw(12) << setprecision(3) << _model._tide << endl;
300
301 //// beg test
302 LOG << "PCO L3: " << setw(12) << setprecision(3)
303 << lc(t_lc::lIF, _model._antPco1, _model._antPco2, 0.0, 0.0) << endl;
304
305 LOG << "WIND L3:" << setw(12) << setprecision(3)
306 << lc(t_lc::lIF, _model._windUp * t_CST::c / _f1,
307 _model._windUp * t_CST::c / _f2, 0.0, 0.0) << endl;
308
309 LOG << "OBS-CMP P3: " << _prn.toString() << " "
310 << setw(12) << setprecision(3) << obsValue(t_lc::cIF) << " "
311 << setw(12) << setprecision(3) << cmpValue(t_lc::cIF) << " "
312 << setw(12) << setprecision(3) << obsValue(t_lc::cIF) - cmpValue(t_lc::cIF) << endl;
313
314 LOG << "OBS-CMP L3: " << _prn.toString() << " "
315 << setw(12) << setprecision(3) << obsValue(t_lc::lIF) << " "
316 << setw(12) << setprecision(3) << cmpValue(t_lc::lIF) << " "
317 << setw(12) << setprecision(3) << obsValue(t_lc::lIF) - cmpValue(t_lc::lIF) << endl;
318
319 LOG << "OBS-CMP MW: " << _prn.toString() << " "
320 << setw(12) << setprecision(3) << obsValue(t_lc::MW) << " "
321 << setw(12) << setprecision(3) << cmpValue(t_lc::MW) << " "
322 << setw(12) << setprecision(3) << obsValue(t_lc::MW) - cmpValue(t_lc::MW) << endl;
323 //// end test
324}
325
326//
327////////////////////////////////////////////////////////////////////////////
328double t_pppSatObs::obsValue(t_lc::type tLC) const {
329
330 if (!_validObs2 && t_lc::need2ndFreq(tLC)) {
331 return 0.0;
332 }
333
334 return this->lc(tLC, _rawL1, _rawL2, _rawC1, _rawC2);
335}
336
337//
338////////////////////////////////////////////////////////////////////////////
339double t_pppSatObs::cmpValueForBanc(t_lc::type tLC) const {
340 return cmpValue(tLC) - _model._rho - _model._sagnac - _model._recClkM;
341}
342
343//
344////////////////////////////////////////////////////////////////////////////
345double t_pppSatObs::cmpValue(t_lc::type tLC) const {
346
347 if (!_validObs2 && t_lc::need2ndFreq(tLC)) {
348 return 0.0;
349 }
350
351 // Non-Dispersive Part
352 // -------------------
353 double nonDisp = _model._rho + _model._recClkM - _model._satClkM
354 + _model._sagnac + _model._antEcc + _model._tropo
355 + _model._tide;
356
357 // Add Dispersive Part
358 // -------------------
359 double L1 = nonDisp + _model._antPco1 - _model._biasL1 + _model._windUp * t_CST::c / _f1;
360 double L2 = nonDisp + _model._antPco2 - _model._biasL2 + _model._windUp * t_CST::c / _f2;
361 double C1 = nonDisp + _model._antPco1 - _model._biasC1;
362 double C2 = nonDisp + _model._antPco2 - _model._biasC2;
363
364 return this->lc(tLC, L1, L2, C1, C2);
365}
366
367//
368////////////////////////////////////////////////////////////////////////////
369double t_pppSatObs::lc(t_lc::type tLC,
370 double L1, double L2, double C1, double C2,
371 ColumnVector* coeff) const {
372
373 if (coeff) {
374 coeff->ReSize(4);
375 (*coeff) = 0.0;
376 }
377
378 if (tLC == t_lc::l1) {
379 if (coeff) (*coeff)(1) = 1.0;
380 return L1;
381 }
382 else if (tLC == t_lc::l2) {
383 if (coeff) (*coeff)(2) = 1.0;
384 return L2;
385 }
386 else if (tLC == t_lc::c1) {
387 if (coeff) (*coeff)(3) = 1.0;
388 return C1;
389 }
390 else if (tLC == t_lc::c2) {
391 if (coeff) (*coeff)(4) = 1.0;
392 return C2;
393 }
394 else if (tLC == t_lc::lIF || tLC == t_lc::cIF) {
395 double a1 = _f1 * _f1 / (_f1 * _f1 - _f2 * _f2);
396 double a2 = -_f2 * _f2 / (_f1 * _f1 - _f2 * _f2);
397 if (tLC == t_lc::lIF) {
398 if (coeff) {
399 (*coeff)(1) = a1;
400 (*coeff)(2) = a2;
401 }
402 return a1 * L1 + a2 * L2;
403 }
404 else {
405 if (coeff) {
406 (*coeff)(3) = a1;
407 (*coeff)(4) = a2;
408 }
409 return a1 * C1 + a2 * C2;
410 }
411 }
412 else if (tLC == t_lc::MW) {
413 double a1 = _f1 / (_f1 - _f2);
414 double a2 = -_f2 / (_f1 - _f2);
415 double a3 = -_f1 / (_f1 + _f2);
416 double a4 = -_f2 / (_f1 + _f2);
417 if (coeff) {
418 (*coeff)(1) = a1;
419 (*coeff)(2) = a2;
420 (*coeff)(3) = a3;
421 (*coeff)(4) = a4;
422 }
423 return a1 * L1 + a2 * L2 + a3 * C1 + a4 * C2;
424 }
425 else if (tLC == t_lc::CL) {
426 if (coeff) {
427 (*coeff)(1) = 0.5;
428 (*coeff)(3) = 0.5;
429 }
430 return (C1 + L1) / 2.0;
431 }
432
433 return 0.0;
434}
435
436//
437////////////////////////////////////////////////////////////////////////////
438double t_pppSatObs::lambda(t_lc::type tLC) const {
439
440 if (tLC == t_lc::l1) {
441 return t_CST::c / _f1;
442 }
443 else if (tLC == t_lc::l2) {
444 return t_CST::c / _f2;
445 }
446 else if (tLC == t_lc::lIF) {
447 return t_CST::c / (_f1 + _f2);
448 }
449 else if (tLC == t_lc::MW) {
450 return t_CST::c / (_f1 - _f2);
451 }
452 else if (tLC == t_lc::CL) {
453 return t_CST::c / _f1 / 2.0;
454 }
455
456 return 0.0;
457}
458
459//
460////////////////////////////////////////////////////////////////////////////
461double t_pppSatObs::sigma(t_lc::type tLC) const {
462
463 ColumnVector sig(4);
464 sig(1) = OPT->_sigmaL1;
465 sig(2) = OPT->_sigmaL1;
466 sig(3) = OPT->_sigmaC1;
467 sig(4) = OPT->_sigmaC1;
468
469 ColumnVector coeff(4);
470 lc(tLC, sig(1), sig(2), sig(3), sig(4), &coeff);
471
472 ColumnVector sp = SP(sig, coeff); // Schur product
473
474 // Elevation-Dependent Weighting
475 // -----------------------------
476 double cEle = 1.0;
477 if ( (OPT->_eleWgtCode && t_lc::includesCode(tLC)) ||
478 (OPT->_eleWgtPhase && t_lc::includesPhase(tLC)) ) {
479 double eleD = eleSat()*180.0/M_PI;
480 double hlp = fabs(90.0 - eleD);
481 cEle = (1.0 + hlp*hlp*hlp*0.000004);
482 }
483
484 return cEle * sp.norm_Frobenius();
485}
486
487//
488////////////////////////////////////////////////////////////////////////////
489double t_pppSatObs::maxRes(t_lc::type tLC) const {
490
491 ColumnVector res(4);
492 res(1) = OPT->_maxResL1;
493 res(2) = OPT->_maxResL1;
494 res(3) = OPT->_maxResC1;
495 res(4) = OPT->_maxResC1;
496
497 ColumnVector coeff(4);
498 lc(tLC, res(1), res(2), res(3), res(4), &coeff);
499
500 ColumnVector sp = SP(res, coeff); // Schur product
501
502 return sp.norm_Frobenius();
503}
504
505//
506////////////////////////////////////////////////////////////////////////////
507void t_pppSatObs::setRes(t_lc::type tLC, double res) {
508 _res[tLC] = res;
509}
510
511//
512////////////////////////////////////////////////////////////////////////////
513double t_pppSatObs::getRes(t_lc::type tLC) const {
514 map<t_lc::type, double>::const_iterator it = _res.find(tLC);
515 if (it != _res.end()) {
516 return it->second;
517 }
518 else {
519 return 0.0;
520 }
521}
Note: See TracBrowser for help on using the repository browser.