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

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