source: ntrip/trunk/BNC/src/pppModel.cpp@ 9336

Last change on this file since 9336 was 9279, checked in by stuerze, 4 years ago

small bug fixed

File size: 21.7 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_astro, t_tides, t_tropo
30 *
31 * Purpose: Observation model
32 *
33 * Author: L. Mervart
34 *
35 * Created: 29-Jul-2014
36 *
37 * Changes:
38 *
39 * -----------------------------------------------------------------------*/
40
41#include <cmath>
42
43#include "pppModel.h"
44
45using namespace BNC_PPP;
46using namespace std;
47
48
49
50Matrix t_astro::rotX(double Angle) {
51 const double C = cos(Angle);
52 const double S = sin(Angle);
53 Matrix UU(3, 3);
54 UU[0][0] = 1.0;
55 UU[0][1] = 0.0;
56 UU[0][2] = 0.0;
57 UU[1][0] = 0.0;
58 UU[1][1] = +C;
59 UU[1][2] = +S;
60 UU[2][0] = 0.0;
61 UU[2][1] = -S;
62 UU[2][2] = +C;
63 return UU;
64}
65
66Matrix t_astro::rotY(double Angle) {
67 const double C = cos(Angle);
68 const double S = sin(Angle);
69 Matrix UU(3, 3);
70 UU[0][0] = +C;
71 UU[0][1] = 0.0;
72 UU[0][2] = -S;
73 UU[1][0] = 0.0;
74 UU[1][1] = 1.0;
75 UU[1][2] = 0.0;
76 UU[2][0] = +S;
77 UU[2][1] = 0.0;
78 UU[2][2] = +C;
79 return UU;
80}
81
82Matrix t_astro::rotZ(double Angle) {
83 const double C = cos(Angle);
84 const double S = sin(Angle);
85 Matrix UU(3, 3);
86 UU[0][0] = +C;
87 UU[0][1] = +S;
88 UU[0][2] = 0.0;
89 UU[1][0] = -S;
90 UU[1][1] = +C;
91 UU[1][2] = 0.0;
92 UU[2][0] = 0.0;
93 UU[2][1] = 0.0;
94 UU[2][2] = 1.0;
95 return UU;
96}
97
98// Greenwich Mean Sidereal Time
99///////////////////////////////////////////////////////////////////////////
100double t_astro::GMST(double Mjd_UT1) {
101
102 const double Secs = 86400.0;
103
104 double Mjd_0 = floor(Mjd_UT1);
105 double UT1 = Secs * (Mjd_UT1 - Mjd_0);
106 double T_0 = (Mjd_0 - MJD_J2000) / 36525.0;
107 double T = (Mjd_UT1 - MJD_J2000) / 36525.0;
108
109 double gmst = 24110.54841 + 8640184.812866 * T_0 + 1.002737909350795 * UT1
110 + (0.093104 - 6.2e-6 * T) * T * T;
111
112 return 2.0 * M_PI * Frac(gmst / Secs);
113}
114
115// Nutation Matrix
116///////////////////////////////////////////////////////////////////////////
117Matrix t_astro::NutMatrix(double Mjd_TT) {
118
119 const double T = (Mjd_TT - MJD_J2000) / 36525.0;
120
121 double ls = 2.0 * M_PI * Frac(0.993133 + 99.997306 * T);
122 double D = 2.0 * M_PI * Frac(0.827362 + 1236.853087 * T);
123 double F = 2.0 * M_PI * Frac(0.259089 + 1342.227826 * T);
124 double N = 2.0 * M_PI * Frac(0.347346 - 5.372447 * T);
125
126 double dpsi = (-17.200 * sin(N) - 1.319 * sin(2 * (F - D + N))
127 - 0.227 * sin(2 * (F + N))
128 + 0.206 * sin(2 * N) + 0.143 * sin(ls)) / RHO_SEC;
129 double deps = (+9.203 * cos(N) + 0.574 * cos(2 * (F - D + N))
130 + 0.098 * cos(2 * (F + N))
131 - 0.090 * cos(2 * N)) / RHO_SEC;
132
133 double eps = 0.4090928 - 2.2696E-4 * T;
134
135 return rotX(-eps - deps) * rotZ(-dpsi) * rotX(+eps);
136}
137
138// Precession Matrix
139///////////////////////////////////////////////////////////////////////////
140Matrix t_astro::PrecMatrix(double Mjd_1, double Mjd_2) {
141
142 const double T = (Mjd_1 - MJD_J2000) / 36525.0;
143 const double dT = (Mjd_2 - Mjd_1) / 36525.0;
144
145 double zeta = ((2306.2181 + (1.39656 - 0.000139 * T) * T) +
146 ((0.30188 - 0.000344 * T) + 0.017998 * dT) * dT) * dT / RHO_SEC;
147 double z = zeta
148 + ((0.79280 + 0.000411 * T) + 0.000205 * dT) * dT * dT / RHO_SEC;
149 double theta = ((2004.3109 - (0.85330 + 0.000217 * T) * T) -
150 ((0.42665 + 0.000217 * T) + 0.041833 * dT) * dT) * dT / RHO_SEC;
151
152 return rotZ(-z) * rotY(theta) * rotZ(-zeta);
153}
154
155// Sun's position
156///////////////////////////////////////////////////////////////////////////
157ColumnVector t_astro::Sun(double Mjd_TT) {
158
159 const double eps = 23.43929111 / RHO_DEG;
160 const double T = (Mjd_TT - MJD_J2000) / 36525.0;
161
162 double M = 2.0 * M_PI * Frac(0.9931267 + 99.9973583 * T);
163 double L = 2.0 * M_PI * Frac(0.7859444 + M / 2.0 / M_PI +
164 (6892.0 * sin(M) + 72.0 * sin(2.0 * M)) / 1296.0e3);
165 double r = 149.619e9 - 2.499e9 * cos(M) - 0.021e9 * cos(2 * M);
166
167 ColumnVector r_Sun(3);
168 r_Sun << r * cos(L) << r * sin(L) << 0.0;
169 r_Sun = rotX(-eps) * r_Sun;
170
171 return rotZ(GMST(Mjd_TT))
172 * NutMatrix(Mjd_TT)
173 * PrecMatrix(MJD_J2000, Mjd_TT)
174 * r_Sun;
175}
176
177// Moon's position
178///////////////////////////////////////////////////////////////////////////
179ColumnVector t_astro::Moon(double Mjd_TT) {
180
181 const double eps = 23.43929111 / RHO_DEG;
182 const double T = (Mjd_TT - MJD_J2000) / 36525.0;
183
184 double L_0 = Frac(0.606433 + 1336.851344 * T);
185 double l = 2.0 * M_PI * Frac(0.374897 + 1325.552410 * T);
186 double lp = 2.0 * M_PI * Frac(0.993133 + 99.997361 * T);
187 double D = 2.0 * M_PI * Frac(0.827361 + 1236.853086 * T);
188 double F = 2.0 * M_PI * Frac(0.259086 + 1342.227825 * T);
189
190 double dL = +22640 * sin(l) - 4586 * sin(l - 2 * D) + 2370 * sin(2 * D)
191 + 769 * sin(2 * l)
192 - 668 * sin(lp) - 412 * sin(2 * F) - 212 * sin(2 * l - 2 * D)
193 - 206 * sin(l + lp - 2 * D)
194 + 192 * sin(l + 2 * D) - 165 * sin(lp - 2 * D) - 125 * sin(D)
195 - 110 * sin(l + lp)
196 + 148 * sin(l - lp) - 55 * sin(2 * F - 2 * D);
197
198 double L = 2.0 * M_PI * Frac(L_0 + dL / 1296.0e3);
199
200 double S = F + (dL + 412 * sin(2 * F) + 541 * sin(lp)) / RHO_SEC;
201 double h = F - 2 * D;
202 double N = -526 * sin(h) + 44 * sin(l + h) - 31 * sin(-l + h)
203 - 23 * sin(lp + h)
204 + 11 * sin(-lp + h) - 25 * sin(-2 * l + F) + 21 * sin(-l + F);
205
206 double B = (18520.0 * sin(S) + N) / RHO_SEC;
207
208 double cosB = cos(B);
209
210 double R = 385000e3 - 20905e3 * cos(l) - 3699e3 * cos(2 * D - l)
211 - 2956e3 * cos(2 * D)
212 - 570e3 * cos(2 * l) + 246e3 * cos(2 * l - 2 * D)
213 - 205e3 * cos(lp - 2 * D)
214 - 171e3 * cos(l + 2 * D) - 152e3 * cos(l + lp - 2 * D);
215
216 ColumnVector r_Moon(3);
217 r_Moon << R * cos(L) * cosB << R * sin(L) * cosB << R * sin(B);
218 r_Moon = rotX(-eps) * r_Moon;
219
220 return rotZ(GMST(Mjd_TT))
221 * NutMatrix(Mjd_TT)
222 * PrecMatrix(MJD_J2000, Mjd_TT)
223 * r_Moon;
224}
225
226// Tidal Correction
227////////////////////////////////////////////////////////////////////////////
228ColumnVector t_tides::earth(const bncTime& time, const ColumnVector& xyz) {
229
230 if (time.undef()) {
231 ColumnVector dX(3);
232 dX = 0.0;
233 return dX;
234 }
235
236 double Mjd = time.mjd() + time.daysec() / 86400.0;
237
238 if (Mjd != _lastMjd) {
239 _lastMjd = Mjd;
240 _xSun = t_astro::Sun(Mjd);
241 _rSun = sqrt(DotProduct(_xSun, _xSun));
242 _xSun /= _rSun;
243 _xMoon = t_astro::Moon(Mjd);
244 _rMoon = sqrt(DotProduct(_xMoon, _xMoon));
245 _xMoon /= _rMoon;
246 }
247
248 double rRec = sqrt(DotProduct(xyz, xyz));
249 ColumnVector xyzUnit = xyz / rRec;
250
251 // Love's Numbers
252 // --------------
253 const double H2 = 0.6078;
254 const double L2 = 0.0847;
255
256 // Tidal Displacement
257 // ------------------
258 double scSun = DotProduct(xyzUnit, _xSun);
259 double scMoon = DotProduct(xyzUnit, _xMoon);
260
261 double p2Sun = 3.0 * (H2 / 2.0 - L2) * scSun * scSun - H2 / 2.0;
262 double p2Moon = 3.0 * (H2 / 2.0 - L2) * scMoon * scMoon - H2 / 2.0;
263
264 double x2Sun = 3.0 * L2 * scSun;
265 double x2Moon = 3.0 * L2 * scMoon;
266
267 const double gmWGS = 398.6005e12;
268 const double gms = 1.3271250e20;
269 const double gmm = 4.9027890e12;
270
271 double facSun = gms / gmWGS *
272 (rRec * rRec * rRec * rRec) / (_rSun * _rSun * _rSun);
273
274 double facMoon = gmm / gmWGS *
275 (rRec * rRec * rRec * rRec) / (_rMoon * _rMoon * _rMoon);
276
277 ColumnVector dX = facSun * (x2Sun * _xSun + p2Sun * xyzUnit)
278 + facMoon * (x2Moon * _xMoon + p2Moon * xyzUnit);
279
280 return dX;
281}
282
283// Constructor
284///////////////////////////////////////////////////////////////////////////
285t_tides::t_tides() {
286 _lastMjd = 0.0;
287 _rSun = 0.0;
288 _rMoon = 0.0;
289 newBlqData = 0;
290}
291
292t_tides::~t_tides() {
293
294 if (newBlqData) {
295 delete newBlqData;
296 }
297
298 QMapIterator<QString, t_blqData*> it(blqMap);
299 while (it.hasNext()) {
300 it.next();
301 delete it.value();
302 }
303}
304
305t_irc t_tides::readBlqFile(const char* fileName) {
306 QFile inFile(fileName);
307 inFile.open(QIODevice::ReadOnly | QIODevice::Text);
308 QTextStream in(&inFile);
309 int row = 0;
310 QString site = QString();
311
312 while (!in.atEnd()) {
313
314 QString line = in.readLine();
315
316 // skip empty lines and comments
317 if (line.indexOf("$$") != -1) {
318 continue;
319 }
320 line = line.trimmed();
321 QTextStream inLine(line.toLatin1(), QIODevice::ReadOnly);
322 switch (row) {
323 case 0:
324 site = line;
325 site = site.toUpper();
326 newBlqData = new t_blqData;
327 newBlqData->amplitudes.ReSize(3, 11);
328 newBlqData->phases.ReSize(3, 11);
329 break;
330 case 1:
331 case 2:
332 case 3:
333 for (int ii = 0; ii < 11; ii++) {
334 inLine >> newBlqData->amplitudes[row - 1][ii];
335 }
336 break;
337 case 4:
338 case 5:
339 for (int ii = 0; ii < 11; ii++) {
340 inLine >> newBlqData->phases[row - 4][ii];
341 }
342 break;
343 case 6:
344 for (int ii = 0; ii < 11; ii++) {
345 inLine >> newBlqData->phases[row - 4][ii];
346 }
347 if (newBlqData && !site.isEmpty()) {
348 blqMap[site] = newBlqData;
349 site = QString();
350 newBlqData = 0;
351 }
352 row = -1;
353 break;
354 }
355 row++;
356 }
357 inFile.close();
358 return success;
359}
360
361ColumnVector t_tides::ocean(const bncTime& time, const ColumnVector& xyz,
362 const std::string& station) {
363 ColumnVector dX(3); dX = 0.0;
364 if (time.undef()) {
365 return dX;
366 }
367 QString stationQ = station.c_str();
368 if (blqMap.find(stationQ) == blqMap.end()) {
369 return dX;
370 }
371 t_blqData* blqSet = blqMap[stationQ]; //printBlqSet(station, blqSet);
372
373 // angular argument: see arg2.f from IERS Conventions software collection
374 double speed[11] = {1.40519e-4, 1.45444e-4, 1.3788e-4, 1.45842e-4, 7.2921e-5,
375 6.7598e-5, 7.2523e-5, 6.4959e-5, 5.3234e-6, 2.6392e-6, 3.982e-7};
376
377 double angfac[4][11];
378 angfac[0][0] = 2.0;
379 angfac[1][0] =-2.0;
380 angfac[2][0] = 0.0;
381 angfac[3][0] = 0.0;
382
383 angfac[0][1] = 0.0;
384 angfac[1][1] = 0.0;
385 angfac[2][1] = 0.0;
386 angfac[3][1] = 0.0;
387
388 angfac[0][2] = 2.0;
389 angfac[1][2] =-3.0;
390 angfac[2][2] = 1.0;
391 angfac[3][2] = 0.0;
392
393 angfac[0][3] = 2.0;
394 angfac[1][3] = 0.0;
395 angfac[2][3] = 0.0;
396 angfac[3][3] = 0.0;
397
398 angfac[0][4] = 1.0;
399 angfac[1][4] = 0.0;
400 angfac[2][4] = 0.0;
401 angfac[3][4] = .25;
402
403 angfac[0][5] = 1.0;
404 angfac[1][5] =-2.0;
405 angfac[2][5] = 0.0;
406 angfac[3][5] =-.25;
407
408 angfac[0][6] =-1.0;
409 angfac[1][6] = 0.0;
410 angfac[2][6] = 0.0;
411 angfac[3][6] =-.25;
412
413 angfac[0][7] = 1.0;
414 angfac[1][7] =-3.0;
415 angfac[2][7] = 1.0;
416 angfac[3][7] =-.25;
417
418 angfac[0][8] = 0.0;
419 angfac[1][8] = 2.0;
420 angfac[2][8] = 0.0;
421 angfac[3][8] = 0.0;
422
423 angfac[0][9] = 0.0;
424 angfac[1][9] = 1.0;
425 angfac[2][9] =-1.0;
426 angfac[3][9] = 0.0;
427
428 angfac[0][10] = 2.0;
429 angfac[1][10] = 0.0;
430 angfac[2][10] = 0.0;
431 angfac[3][10] = 0.0;
432
433 double twopi = 6.283185307179586476925287e0;
434 double dtr = 0.0174532925199;
435
436 // fractional part of the day in seconds
437 unsigned int year, month, day;
438 time.civil_date(year, month, day);
439 int iyear = year - 2000;
440 QDateTime datTim = QDateTime::fromString(QString::fromStdString(time.datestr()), Qt::ISODate);
441 int doy = datTim.date().dayOfYear();
442 double fday = time.daysec();
443 int icapd = doy + 365 * (iyear - 75) + ((iyear - 73) / 4);
444 double capt = (icapd * 1.000000035 + 27392.500528) / 36525.0;
445
446 // mean longitude of the sun at the beginning of the day
447 double h0 = (279.69668e0 + (36000.768930485e0 + 3.03e-4 * capt) * capt) * dtr;
448
449 // mean longitude of moon at the beginning of the day
450 double s0 = (((1.9e-6 * capt - .001133e0) * capt + 481267.88314137e0) * capt + 270.434358e0) * dtr;
451
452 // mean longitude of lunar perigee at the beginning of the day
453 double p0 = (((-1.2e-5 * capt - .010325e0) * capt + 4069.0340329577e0) * capt + 334.329653e0) * dtr;
454
455 // tidal angle arguments
456 double angle[11];
457 for (int k = 0; k < 11; ++k) {
458 angle[k] = speed[k] * fday
459 + angfac[0][k] * h0
460 + angfac[1][k] * s0
461 + angfac[2][k] * p0
462 + angfac[3][k] * twopi;
463 angle[k] = fmod(angle[k], twopi);
464 if (angle[k] < 0.0) {
465 angle[k] += twopi;
466 }
467 }
468
469 // displacement by 11 constituents
470 ColumnVector rwsSta(3); rwsSta = 0.0; // radial, west, south
471 for (int rr = 0; rr < 3; rr++) {
472 for (int cc = 0; cc < 11; cc++) {
473 rwsSta[rr] += blqSet->amplitudes[rr][cc] * cos((angle[cc] - (blqSet->phases[rr][cc]/RHO_DEG)));
474 }
475 }
476
477 // neu2xyz
478 ColumnVector dneu(3); // neu
479 dneu[0] = -rwsSta[2];
480 dneu[1] = -rwsSta[1];
481 dneu[2] = rwsSta[0];
482 double recEll[3]; xyz2ell(xyz.data(), recEll) ;
483 neu2xyz(recEll, dneu.data(), dX.data());
484
485 return dX;
486}
487
488// Print
489////////////////////////////////////////////////////////////////////////////
490void t_tides::printAllBlqSets() const {
491
492 QMapIterator<QString, t_blqData*> it(blqMap);
493 while (it.hasNext()) {
494 it.next();
495 t_blqData* blq = it.value();
496 QString site = it.key();
497 cout << site.toStdString().c_str() << "\n===============\n";
498 for (int rr = 0; rr < 3; rr++) {
499 for (int cc = 0; cc < 11; cc++) {
500 cout << blq->amplitudes[rr][cc] << " ";
501 }
502 cout << endl;
503 }
504 for (int rr = 0; rr < 3; rr++) {
505 for (int cc = 0; cc < 11; cc++) {
506 cout << blq->phases[rr][cc] << " ";
507 }
508 cout << endl;
509 }
510 }
511}
512
513// Print
514////////////////////////////////////////////////////////////////////////////
515void t_tides::printBlqSet(const std::string& station, t_blqData* blq) {
516 cout << station << endl;
517 for (int rr = 0; rr < 3; rr++) {
518 for (int cc = 0; cc < 11; cc++) {
519 cout << blq->amplitudes[rr][cc] << " ";
520 }
521 cout << endl;
522 }
523 for (int rr = 0; rr < 3; rr++) {
524 for (int cc = 0; cc < 11; cc++) {
525 cout << blq->phases[rr][cc] << " ";
526 }
527 cout << endl;
528 }
529}
530
531// Constructor
532///////////////////////////////////////////////////////////////////////////
533t_windUp::t_windUp() {
534 for (unsigned ii = 0; ii <= t_prn::MAXPRN; ii++) {
535 sumWind[ii] = 0.0;
536 lastEtime[ii] = 0.0;
537 }
538}
539
540// Phase Wind-Up Correction
541///////////////////////////////////////////////////////////////////////////
542double t_windUp::value(const bncTime& etime, const ColumnVector& rRec,
543 t_prn prn, const ColumnVector& rSat, bool ssr,
544 double yaw, const ColumnVector& vSat) {
545
546 if (etime.mjddec() != lastEtime[prn.toInt()]) {
547
548 // Unit Vector GPS Satellite --> Receiver
549 // --------------------------------------
550 ColumnVector rho = rRec - rSat;
551 rho /= rho.NormFrobenius();
552
553 // GPS Satellite unit Vectors sz, sy, sx
554 // -------------------------------------
555 ColumnVector sHlp;
556 if (!ssr) {
557 sHlp = t_astro::Sun(etime.mjddec());
558 }
559 else {
560 ColumnVector Omega(3);
561 Omega[0] = 0.0;
562 Omega[1] = 0.0;
563 Omega[2] = t_CST::omega;
564 sHlp = vSat + crossproduct(Omega, rSat);
565 }
566 sHlp /= sHlp.NormFrobenius();
567
568 ColumnVector sz = -rSat / rSat.NormFrobenius();
569 ColumnVector sy = crossproduct(sz, sHlp);
570 ColumnVector sx = crossproduct(sy, sz);
571
572 if (ssr) {
573 // Yaw angle consideration
574 Matrix SXYZ(3, 3);
575 SXYZ.Column(1) = sx;
576 SXYZ.Column(2) = sy;
577 SXYZ.Column(3) = sz;
578 SXYZ = DotProduct(t_astro::rotZ(yaw), SXYZ);
579 sx = SXYZ.Column(1);
580 sy = SXYZ.Column(2);
581 sz = SXYZ.Column(3);
582 }
583 // Effective Dipole of the GPS Satellite Antenna
584 // ---------------------------------------------
585 ColumnVector dipSat = sx - rho * DotProduct(rho, sx)
586 - crossproduct(rho, sy);
587
588 // Receiver unit Vectors rx, ry
589 // ----------------------------
590 ColumnVector rx(3);
591 ColumnVector ry(3);
592 double recEll[3];
593 xyz2ell(rRec.data(), recEll);
594 double neu[3];
595
596 neu[0] = 1.0;
597 neu[1] = 0.0;
598 neu[2] = 0.0;
599 neu2xyz(recEll, neu, rx.data());
600
601 neu[0] = 0.0;
602 neu[1] = -1.0;
603 neu[2] = 0.0;
604 neu2xyz(recEll, neu, ry.data());
605
606 // Effective Dipole of the Receiver Antenna
607 // ----------------------------------------
608 ColumnVector dipRec = rx - rho * DotProduct(rho, rx)
609 + crossproduct(rho, ry);
610
611 // Resulting Effect
612 // ----------------
613 double alpha = DotProduct(dipSat, dipRec)
614 / (dipSat.NormFrobenius() * dipRec.NormFrobenius());
615
616 if (alpha > 1.0)
617 alpha = 1.0;
618 if (alpha < -1.0)
619 alpha = -1.0;
620
621 double dphi = acos(alpha) / 2.0 / M_PI; // in cycles
622
623 if (DotProduct(rho, crossproduct(dipSat, dipRec)) < 0.0) {
624 dphi = -dphi;
625 }
626
627 if (lastEtime[prn.toInt()] == 0.0) {
628 sumWind[prn.toInt()] = dphi;
629 }
630 else {
631 sumWind[prn.toInt()] = nint(sumWind[prn.toInt()] - dphi) + dphi;
632 }
633
634 lastEtime[prn.toInt()] = etime.mjddec();
635 }
636
637 return sumWind[prn.toInt()];
638}
639
640// Tropospheric Model (Saastamoinen)
641////////////////////////////////////////////////////////////////////////////
642double t_tropo::delay_saast(const ColumnVector& xyz, double Ele) {
643
644 Tracer tracer("bncModel::delay_saast");
645
646 if (xyz[0] == 0.0 && xyz[1] == 0.0 && xyz[2] == 0.0) {
647 return 0.0;
648 }
649
650 double ell[3];
651 xyz2ell(xyz.data(), ell);
652 double height = ell[2];
653 // Prevent pp from causing segmentation fault (Loukis)
654 if (height > 40000.0 ) {
655 return 0.000000001;
656 }
657
658 double pp = 1013.25 * pow(1.0 - 2.26e-5 * height, 5.225);
659 double TT = 18.0 - height * 0.0065 + 273.15;
660 double hh = 50.0 * exp(-6.396e-4 * height);
661 double ee = hh / 100.0
662 * exp(-37.2465 + 0.213166 * TT - 0.000256908 * TT * TT);
663
664 double h_km = height / 1000.0;
665
666 if (h_km < 0.0)
667 h_km = 0.0;
668 if (h_km > 5.0)
669 h_km = 5.0;
670 int ii = int(h_km + 1);
671 if (ii > 5)
672 ii = 5;
673 double href = ii - 1;
674
675 double bCor[6];
676 bCor[0] = 1.156;
677 bCor[1] = 1.006;
678 bCor[2] = 0.874;
679 bCor[3] = 0.757;
680 bCor[4] = 0.654;
681 bCor[5] = 0.563;
682
683 double BB = bCor[ii - 1] + (bCor[ii] - bCor[ii - 1]) * (h_km - href);
684
685 double zen = M_PI / 2.0 - Ele;
686
687 return (0.002277 / cos(zen))
688 * (pp + ((1255.0 / TT) + 0.05) * ee - BB * (tan(zen) * tan(zen)));
689}
690
691// Constructor
692///////////////////////////////////////////////////////////////////////////
693t_iono::t_iono() {
694 _psiPP = _phiPP = _lambdaPP = _lonS = 0.0;
695}
696
697t_iono::~t_iono() {
698}
699
700double t_iono::stec(const t_vTec* vTec, double signalPropagationTime,
701 const ColumnVector& rSat, const bncTime& epochTime,
702 const ColumnVector& xyzSta) {
703
704 // Latitude, longitude, height are defined with respect to a spherical earth model
705 // -------------------------------------------------------------------------------
706 ColumnVector geocSta(3);
707 if (xyz2geoc(xyzSta.data(), geocSta.data()) != success) {
708 return 0.0;
709 }
710
711 // satellite position rotated to the epoch of signal reception
712 // -----------------------------------------------------------
713 ColumnVector xyzSat(3);
714 double omegaZ = t_CST::omega * signalPropagationTime;
715 xyzSat[0] = rSat[0] * cos(omegaZ) + rSat[1] * sin(omegaZ);
716 xyzSat[1] = rSat[1] * cos(omegaZ) - rSat[0] * sin(omegaZ);
717 xyzSat[2] = rSat[2];
718
719 // elevation and azimuth with respect to a spherical earth model
720 // -------------------------------------------------------------
721 ColumnVector rhoV = xyzSat - xyzSta;
722 double rho = rhoV.NormFrobenius();
723 ColumnVector neu(3);
724 xyz2neu(geocSta.data(), rhoV.data(), neu.data());
725 double sphEle = acos(sqrt(neu[0] * neu[0] + neu[1] * neu[1]) / rho);
726 if (neu[2] < 0) {
727 sphEle *= -1.0;
728 }
729 double sphAzi = atan2(neu[1], neu[0]);
730
731 double epoch = fmod(epochTime.gpssec(), 86400.0);
732
733 double stec = 0.0;
734 for (unsigned ii = 0; ii < vTec->_layers.size(); ii++) {
735 piercePoint(vTec->_layers[ii]._height, epoch, geocSta.data(), sphEle,
736 sphAzi);
737 double vtec = vtecSingleLayerContribution(vTec->_layers[ii]);
738 stec += vtec * sin(sphEle + _psiPP);
739 }
740 return stec;
741}
742
743double t_iono::vtecSingleLayerContribution(const t_vTecLayer& vTecLayer) {
744
745 double vtec = 0.0;
746 int N = vTecLayer._C.Nrows() - 1;
747 int M = vTecLayer._C.Ncols() - 1;
748 double fac;
749
750 for (int n = 0; n <= N; n++) {
751 for (int m = 0; m <= min(n, M); m++) {
752 double pnm = associatedLegendreFunction(n, m, sin(_phiPP));
753 double a = factorial(n - m);
754 double b = factorial(n + m);
755 if (m == 0) {
756 fac = sqrt(2.0 * n + 1);
757 }
758 else {
759 fac = sqrt(2.0 * (2.0 * n + 1) * a / b);
760 }
761 pnm *= fac;
762 double Cnm_mlambda = vTecLayer._C[n][m] * cos(m * _lonS);
763 double Snm_mlambda = vTecLayer._S[n][m] * sin(m * _lonS);
764 vtec += (Snm_mlambda + Cnm_mlambda) * pnm;
765 }
766 }
767
768 if (vtec < 0.0) {
769 vtec = 0.0;
770 }
771
772 return vtec;
773}
774
775void t_iono::piercePoint(double layerHeight, double epoch,
776 const double* geocSta,
777 double sphEle, double sphAzi) {
778
779 double q = (t_CST::rgeoc + geocSta[2]) / (t_CST::rgeoc + layerHeight);
780
781 _psiPP = M_PI / 2 - sphEle - asin(q * cos(sphEle));
782
783 _phiPP = asin(
784 sin(geocSta[0]) * cos(_psiPP)
785 + cos(geocSta[0]) * sin(_psiPP) * cos(sphAzi));
786
787 if (((geocSta[0] * 180.0 / M_PI > 0)
788 && (tan(_psiPP) * cos(sphAzi) > tan(M_PI / 2 - geocSta[0])))
789 ||
790 ((geocSta[0] * 180.0 / M_PI < 0)
791 && (-(tan(_psiPP) * cos(sphAzi)) > tan(M_PI / 2 + geocSta[0])))) {
792 _lambdaPP = geocSta[1] + M_PI
793 - asin((sin(_psiPP) * sin(sphAzi) / cos(_phiPP)));
794 }
795 else {
796 _lambdaPP = geocSta[1] + asin((sin(_psiPP) * sin(sphAzi) / cos(_phiPP)));
797 }
798
799 _lonS = fmod((_lambdaPP + (epoch - 50400) * M_PI / 43200), 2 * M_PI);
800
801 return;
802}
803
Note: See TracBrowser for help on using the repository browser.