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

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