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

Last change on this file since 8677 was 8619, checked in by stuerze, 6 years ago

special ssr wind up computation is added
File size: 17.4 KB
Line 
1
2// Part of BNC, a utility for retrieving decoding and
3// converting GNSS data streams from NTRIP broadcasters.
4//
5// Copyright (C) 2007
6// German Federal Agency for Cartography and Geodesy (BKG)
7// http://www.bkg.bund.de
8// Czech Technical University Prague, Department of Geodesy
9// http://www.fsv.cvut.cz
10//
11// Email: euref-ip@bkg.bund.de
12//
13// This program is free software; you can redistribute it and/or
14// modify it under the terms of the GNU General Public License
15// as published by the Free Software Foundation, version 2.
16//
17// This program is distributed in the hope that it will be useful,
18// but WITHOUT ANY WARRANTY; without even the implied warranty of
19// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
20// GNU General Public License for more details.
21//
22// You should have received a copy of the GNU General Public License
23// along with this program; if not, write to the Free Software
24// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
25
26/* -------------------------------------------------------------------------
27 * BKG NTRIP Client
28 * -------------------------------------------------------------------------
29 *
30 * Class: t_astro, t_tides, t_tropo
31 *
32 * Purpose: Observation model
33 *
34 * Author: L. Mervart
35 *
36 * Created: 29-Jul-2014
37 *
38 * Changes:
39 *
40 * -----------------------------------------------------------------------*/
41
42
43#include <cmath>
44
45#include "pppModel.h"
46
47using namespace BNC_PPP;
48using namespace std;
49
50const double t_astro::RHO_DEG = 180.0 / M_PI;
51const double t_astro::RHO_SEC = 3600.0 * 180.0 / M_PI;
52const double t_astro::MJD_J2000 = 51544.5;
53
54Matrix t_astro::rotX(double Angle) {
55 const double C = cos(Angle);
56 const double S = sin(Angle);
57 Matrix UU(3,3);
58 UU[0][0] = 1.0; UU[0][1] = 0.0; UU[0][2] = 0.0;
59 UU[1][0] = 0.0; UU[1][1] = +C; UU[1][2] = +S;
60 UU[2][0] = 0.0; UU[2][1] = -S; UU[2][2] = +C;
61 return UU;
62}
63
64Matrix t_astro::rotY(double Angle) {
65 const double C = cos(Angle);
66 const double S = sin(Angle);
67 Matrix UU(3,3);
68 UU[0][0] = +C; UU[0][1] = 0.0; UU[0][2] = -S;
69 UU[1][0] = 0.0; UU[1][1] = 1.0; UU[1][2] = 0.0;
70 UU[2][0] = +S; UU[2][1] = 0.0; UU[2][2] = +C;
71 return UU;
72}
73
74Matrix t_astro::rotZ(double Angle) {
75 const double C = cos(Angle);
76 const double S = sin(Angle);
77 Matrix UU(3,3);
78 UU[0][0] = +C; UU[0][1] = +S; UU[0][2] = 0.0;
79 UU[1][0] = -S; UU[1][1] = +C; UU[1][2] = 0.0;
80 UU[2][0] = 0.0; UU[2][1] = 0.0; UU[2][2] = 1.0;
81 return UU;
82}
83
84// Greenwich Mean Sidereal Time
85///////////////////////////////////////////////////////////////////////////
86double t_astro::GMST(double Mjd_UT1) {
87
88 const double Secs = 86400.0;
89
90 double Mjd_0 = floor(Mjd_UT1);
91 double UT1 = Secs*(Mjd_UT1-Mjd_0);
92 double T_0 = (Mjd_0 -MJD_J2000)/36525.0;
93 double T = (Mjd_UT1-MJD_J2000)/36525.0;
94
95 double gmst = 24110.54841 + 8640184.812866*T_0 + 1.002737909350795*UT1
96 + (0.093104-6.2e-6*T)*T*T;
97
98 return 2.0*M_PI*Frac(gmst/Secs);
99}
100
101// Nutation Matrix
102///////////////////////////////////////////////////////////////////////////
103Matrix t_astro::NutMatrix(double Mjd_TT) {
104
105 const double T = (Mjd_TT-MJD_J2000)/36525.0;
106
107 double ls = 2.0*M_PI*Frac(0.993133+ 99.997306*T);
108 double D = 2.0*M_PI*Frac(0.827362+1236.853087*T);
109 double F = 2.0*M_PI*Frac(0.259089+1342.227826*T);
110 double N = 2.0*M_PI*Frac(0.347346- 5.372447*T);
111
112 double dpsi = ( -17.200*sin(N) - 1.319*sin(2*(F-D+N)) - 0.227*sin(2*(F+N))
113 + 0.206*sin(2*N) + 0.143*sin(ls) ) / RHO_SEC;
114 double deps = ( + 9.203*cos(N) + 0.574*cos(2*(F-D+N)) + 0.098*cos(2*(F+N))
115 - 0.090*cos(2*N) ) / RHO_SEC;
116
117 double eps = 0.4090928-2.2696E-4*T;
118
119 return rotX(-eps-deps)*rotZ(-dpsi)*rotX(+eps);
120}
121
122// Precession Matrix
123///////////////////////////////////////////////////////////////////////////
124Matrix t_astro::PrecMatrix(double Mjd_1, double Mjd_2) {
125
126 const double T = (Mjd_1-MJD_J2000)/36525.0;
127 const double dT = (Mjd_2-Mjd_1)/36525.0;
128
129 double zeta = ( (2306.2181+(1.39656-0.000139*T)*T)+
130 ((0.30188-0.000344*T)+0.017998*dT)*dT )*dT/RHO_SEC;
131 double z = zeta + ( (0.79280+0.000411*T)+0.000205*dT)*dT*dT/RHO_SEC;
132 double theta = ( (2004.3109-(0.85330+0.000217*T)*T)-
133 ((0.42665+0.000217*T)+0.041833*dT)*dT )*dT/RHO_SEC;
134
135 return rotZ(-z) * rotY(theta) * rotZ(-zeta);
136}
137
138// Sun's position
139///////////////////////////////////////////////////////////////////////////
140ColumnVector t_astro::Sun(double Mjd_TT) {
141
142 const double eps = 23.43929111/RHO_DEG;
143 const double T = (Mjd_TT-MJD_J2000)/36525.0;
144
145 double M = 2.0*M_PI * Frac ( 0.9931267 + 99.9973583*T);
146 double L = 2.0*M_PI * Frac ( 0.7859444 + M/2.0/M_PI +
147 (6892.0*sin(M)+72.0*sin(2.0*M)) / 1296.0e3);
148 double r = 149.619e9 - 2.499e9*cos(M) - 0.021e9*cos(2*M);
149
150 ColumnVector r_Sun(3);
151 r_Sun << r*cos(L) << r*sin(L) << 0.0; r_Sun = rotX(-eps) * r_Sun;
152
153 return rotZ(GMST(Mjd_TT))
154 * NutMatrix(Mjd_TT)
155 * PrecMatrix(MJD_J2000, Mjd_TT)
156 * r_Sun;
157}
158
159// Moon's position
160///////////////////////////////////////////////////////////////////////////
161ColumnVector t_astro::Moon(double Mjd_TT) {
162
163 const double eps = 23.43929111/RHO_DEG;
164 const double T = (Mjd_TT-MJD_J2000)/36525.0;
165
166 double L_0 = Frac ( 0.606433 + 1336.851344*T );
167 double l = 2.0*M_PI*Frac ( 0.374897 + 1325.552410*T );
168 double lp = 2.0*M_PI*Frac ( 0.993133 + 99.997361*T );
169 double D = 2.0*M_PI*Frac ( 0.827361 + 1236.853086*T );
170 double F = 2.0*M_PI*Frac ( 0.259086 + 1342.227825*T );
171
172 double dL = +22640*sin(l) - 4586*sin(l-2*D) + 2370*sin(2*D) + 769*sin(2*l)
173 -668*sin(lp) - 412*sin(2*F) - 212*sin(2*l-2*D)- 206*sin(l+lp-2*D)
174 +192*sin(l+2*D) - 165*sin(lp-2*D) - 125*sin(D) - 110*sin(l+lp)
175 +148*sin(l-lp) - 55*sin(2*F-2*D);
176
177 double L = 2.0*M_PI * Frac( L_0 + dL/1296.0e3 );
178
179 double S = F + (dL+412*sin(2*F)+541*sin(lp)) / RHO_SEC;
180 double h = F-2*D;
181 double N = -526*sin(h) + 44*sin(l+h) - 31*sin(-l+h) - 23*sin(lp+h)
182 +11*sin(-lp+h) - 25*sin(-2*l+F) + 21*sin(-l+F);
183
184 double B = ( 18520.0*sin(S) + N ) / RHO_SEC;
185
186 double cosB = cos(B);
187
188 double R = 385000e3 - 20905e3*cos(l) - 3699e3*cos(2*D-l) - 2956e3*cos(2*D)
189 -570e3*cos(2*l) + 246e3*cos(2*l-2*D) - 205e3*cos(lp-2*D)
190 -171e3*cos(l+2*D) - 152e3*cos(l+lp-2*D);
191
192 ColumnVector r_Moon(3);
193 r_Moon << R*cos(L)*cosB << R*sin(L)*cosB << R*sin(B);
194 r_Moon = rotX(-eps) * r_Moon;
195
196 return rotZ(GMST(Mjd_TT))
197 * NutMatrix(Mjd_TT)
198 * PrecMatrix(MJD_J2000, Mjd_TT)
199 * r_Moon;
200}
201
202// Tidal Correction
203////////////////////////////////////////////////////////////////////////////
204ColumnVector t_tides::displacement(const bncTime& time, const ColumnVector& xyz) {
205
206 if (time.undef()) {
207 ColumnVector dX(3); dX = 0.0;
208 return dX;
209 }
210
211 double Mjd = time.mjd() + time.daysec() / 86400.0;
212
213 if (Mjd != _lastMjd) {
214 _lastMjd = Mjd;
215 _xSun = t_astro::Sun(Mjd);
216 _rSun = sqrt(DotProduct(_xSun,_xSun));
217 _xSun /= _rSun;
218 _xMoon = t_astro::Moon(Mjd);
219 _rMoon = sqrt(DotProduct(_xMoon,_xMoon));
220 _xMoon /= _rMoon;
221 }
222
223 double rRec = sqrt(DotProduct(xyz, xyz));
224 ColumnVector xyzUnit = xyz / rRec;
225
226 // Love's Numbers
227 // --------------
228 const double H2 = 0.6078;
229 const double L2 = 0.0847;
230
231 // Tidal Displacement
232 // ------------------
233 double scSun = DotProduct(xyzUnit, _xSun);
234 double scMoon = DotProduct(xyzUnit, _xMoon);
235
236 double p2Sun = 3.0 * (H2/2.0-L2) * scSun * scSun - H2/2.0;
237 double p2Moon = 3.0 * (H2/2.0-L2) * scMoon * scMoon - H2/2.0;
238
239 double x2Sun = 3.0 * L2 * scSun;
240 double x2Moon = 3.0 * L2 * scMoon;
241
242 const double gmWGS = 398.6005e12;
243 const double gms = 1.3271250e20;
244 const double gmm = 4.9027890e12;
245
246 double facSun = gms / gmWGS *
247 (rRec * rRec * rRec * rRec) / (_rSun * _rSun * _rSun);
248
249 double facMoon = gmm / gmWGS *
250 (rRec * rRec * rRec * rRec) / (_rMoon * _rMoon * _rMoon);
251
252 ColumnVector dX = facSun * (x2Sun * _xSun + p2Sun * xyzUnit) +
253 facMoon * (x2Moon * _xMoon + p2Moon * xyzUnit);
254
255 return dX;
256}
257
258// Constructor
259///////////////////////////////////////////////////////////////////////////
260t_loading::t_loading(const QString& fileName) {
261
262 newBlqData = 0;
263 readFile(fileName);
264 printAll();
265}
266
267t_loading::~t_loading() {
268
269 if (newBlqData) {
270 delete newBlqData;
271 }
272
273 QMapIterator<QString, t_blqData*> it(blqMap);
274 while (it.hasNext()) {
275 it.next();
276 delete it.value();
277 }
278}
279
280t_irc t_loading::readFile(const QString& fileName) {
281 QFile inFile(fileName);
282 inFile.open(QIODevice::ReadOnly | QIODevice::Text);
283 QTextStream in(&inFile);
284 int row = 0;
285 QString site = QString();
286
287 while ( !in.atEnd() ) {
288
289 QString line = in.readLine();
290
291 // skip empty lines and comments
292 if (line.indexOf("$$") != -1 || line.size() == 0) {
293 continue;
294 }
295
296 line = line.trimmed();
297 QTextStream inLine(line.toLatin1(), QIODevice::ReadOnly);
298
299 switch (row) {
300 case 0:
301 site = line;
302 site = site.toUpper();
303 if (!newBlqData) {
304 newBlqData = new t_blqData;
305 newBlqData->amplitudes.ReSize(3,11);
306 newBlqData->phases.ReSize(3,11);
307 }
308 break;
309 case 1: case 2: case 3:
310 for (int ii = 0; ii < 11; ii++) {
311 inLine >> newBlqData->amplitudes[row-1][ii];
312 }
313 break;
314 case 4: case 5: case 6:
315 for (int ii = 0; ii < 11; ii++) {
316 inLine >> newBlqData->phases[row-4][ii];
317 }
318 break;
319 case 7:
320 if (newBlqData && !site.isEmpty()) {
321 blqMap[site] = newBlqData;
322 site = QString();
323 row = -1;
324 newBlqData = 0;
325 }
326 break;
327 }
328 row++;
329 }
330 inFile.close();
331 return success;
332}
333
334// Print
335////////////////////////////////////////////////////////////////////////////
336void t_loading::printAll() const {
337
338 QMapIterator<QString, t_blqData*> it(blqMap);
339 while (it.hasNext()) {
340 it.next();
341 t_blqData* blq = it.value();
342 QString site = it.key();
343 cout << site.toStdString().c_str() << "\n";
344 for (int rr = 0; rr < 3; rr++) {
345 for (int cc = 0; cc < 11; cc++) {
346 cout << blq->amplitudes[rr][cc] << " ";
347 }
348 cout << endl;
349 }
350 for (int rr = 0; rr < 3; rr++) {
351 for (int cc = 0; cc < 11; cc++) {
352 cout << blq->phases[rr][cc] << " ";
353 }
354 cout << endl;
355 }
356 }
357}
358
359// Constructor
360///////////////////////////////////////////////////////////////////////////
361t_windUp::t_windUp() {
362 for (unsigned ii = 0; ii <= t_prn::MAXPRN; ii++) {
363 sumWind[ii] = 0.0;
364 lastEtime[ii] = 0.0;
365 }
366}
367
368// Phase Wind-Up Correction
369///////////////////////////////////////////////////////////////////////////
370double t_windUp::value(const bncTime& etime, const ColumnVector& rRec,
371 t_prn prn, const ColumnVector& rSat, bool ssr,
372 double yaw, const ColumnVector& vSat) {
373
374 if (etime.mjddec() != lastEtime[prn.toInt()]) {
375
376 // Unit Vector GPS Satellite --> Receiver
377 // --------------------------------------
378 ColumnVector rho = rRec - rSat;
379 rho /= rho.norm_Frobenius();
380
381 // GPS Satellite unit Vectors sz, sy, sx
382 // -------------------------------------
383 ColumnVector sHlp;
384 if (!ssr) {
385 sHlp = t_astro::Sun(etime.mjddec());
386 }
387 else {
388 ColumnVector Omega(3);
389 Omega[0] = 0.0;
390 Omega[1] = 0.0;
391 Omega[2] = t_CST::omega;
392 sHlp = vSat + crossproduct(Omega, rSat);
393 }
394 sHlp /= sHlp.norm_Frobenius();
395
396 ColumnVector sz = -rSat / rSat.norm_Frobenius();
397 ColumnVector sy = crossproduct(sz, sHlp);
398 ColumnVector sx = crossproduct(sy, sz);
399
400 // Yaw consideration
401 sx = t_astro::rotZ(yaw) * sx;
402
403 // Effective Dipole of the GPS Satellite Antenna
404 // ---------------------------------------------
405 ColumnVector dipSat = sx - rho * DotProduct(rho,sx) - crossproduct(rho, sy);
406
407 // Receiver unit Vectors rx, ry
408 // ----------------------------
409 ColumnVector rx(3);
410 ColumnVector ry(3);
411 double recEll[3]; xyz2ell(rRec.data(), recEll) ;
412 double neu[3];
413
414 neu[0] = 1.0;
415 neu[1] = 0.0;
416 neu[2] = 0.0;
417 neu2xyz(recEll, neu, rx.data());
418
419 neu[0] = 0.0;
420 neu[1] = -1.0;
421 neu[2] = 0.0;
422 neu2xyz(recEll, neu, ry.data());
423
424 // Effective Dipole of the Receiver Antenna
425 // ----------------------------------------
426 ColumnVector dipRec = rx - rho * DotProduct(rho,rx) + crossproduct(rho, ry);
427
428 // Resulting Effect
429 // ----------------
430 double alpha = DotProduct(dipSat,dipRec) /
431 (dipSat.norm_Frobenius() * dipRec.norm_Frobenius());
432/*
433 if (alpha > 1.0) alpha = 1.0;
434 if (alpha < -1.0) alpha = -1.0;
435*/
436 double dphi = acos(alpha) / 2.0 / M_PI; // in cycles
437
438 if ( DotProduct(rho, crossproduct(dipSat, dipRec)) < 0.0 ) {
439 dphi = -dphi;
440 }
441
442 if (lastEtime[prn.toInt()] == 0.0) {
443 sumWind[prn.toInt()] = dphi;
444 }
445 else {
446 sumWind[prn.toInt()] = nint(sumWind[prn.toInt()] - dphi) + dphi;
447 }
448
449 lastEtime[prn.toInt()] = etime.mjddec();
450 }
451
452 return sumWind[prn.toInt()];
453}
454
455// Tropospheric Model (Saastamoinen)
456////////////////////////////////////////////////////////////////////////////
457double t_tropo::delay_saast(const ColumnVector& xyz, double Ele) {
458
459 Tracer tracer("bncModel::delay_saast");
460
461 if (xyz[0] == 0.0 && xyz[1] == 0.0 && xyz[2] == 0.0) {
462 return 0.0;
463 }
464
465 double ell[3];
466 xyz2ell(xyz.data(), ell);
467 double height = ell[2];
468
469 double pp = 1013.25 * pow(1.0 - 2.26e-5 * height, 5.225);
470 double TT = 18.0 - height * 0.0065 + 273.15;
471 double hh = 50.0 * exp(-6.396e-4 * height);
472 double ee = hh / 100.0 * exp(-37.2465 + 0.213166*TT - 0.000256908*TT*TT);
473
474 double h_km = height / 1000.0;
475
476 if (h_km < 0.0) h_km = 0.0;
477 if (h_km > 5.0) h_km = 5.0;
478 int ii = int(h_km + 1); if (ii > 5) ii = 5;
479 double href = ii - 1;
480
481 double bCor[6];
482 bCor[0] = 1.156;
483 bCor[1] = 1.006;
484 bCor[2] = 0.874;
485 bCor[3] = 0.757;
486 bCor[4] = 0.654;
487 bCor[5] = 0.563;
488
489 double BB = bCor[ii-1] + (bCor[ii]-bCor[ii-1]) * (h_km - href);
490
491 double zen = M_PI/2.0 - Ele;
492
493 return (0.002277/cos(zen)) * (pp + ((1255.0/TT)+0.05)*ee - BB*(tan(zen)*tan(zen)));
494}
495
496// Constructor
497///////////////////////////////////////////////////////////////////////////
498t_iono::t_iono() {
499 _psiPP = _phiPP = _lambdaPP = _lonS = 0.0;
500}
501
502t_iono::~t_iono() {}
503
504double t_iono::stec(const t_vTec* vTec, double signalPropagationTime,
505 const ColumnVector& rSat, const bncTime& epochTime,
506 const ColumnVector& xyzSta) {
507
508 // Latitude, longitude, height are defined with respect to a spherical earth model
509 // -------------------------------------------------------------------------------
510 ColumnVector geocSta(3);
511 if (xyz2geoc(xyzSta.data(), geocSta.data()) != success) {
512 return 0.0;
513 }
514
515 // satellite position rotated to the epoch of signal reception
516 // -----------------------------------------------------------
517 ColumnVector xyzSat(3);
518 double omegaZ = t_CST::omega * signalPropagationTime;
519 xyzSat[0] = rSat[0] * cos(omegaZ) + rSat[1] * sin(omegaZ);
520 xyzSat[1] = rSat[1] * cos(omegaZ) - rSat[0] * sin(omegaZ);
521 xyzSat[2] = rSat[2];
522
523 // elevation and azimuth with respect to a spherical earth model
524 // -------------------------------------------------------------
525 ColumnVector rhoV = xyzSat - xyzSta;
526 double rho = rhoV.norm_Frobenius();
527 ColumnVector neu(3);
528 xyz2neu(geocSta.data(), rhoV.data(), neu.data());
529 double sphEle = acos( sqrt(neu[0]*neu[0] + neu[1]*neu[1]) / rho );
530 if (neu[2] < 0) {
531 sphEle *= -1.0;
532 }
533 double sphAzi = atan2(neu[1], neu[0]);
534
535 double epoch = fmod(epochTime.gpssec(), 86400.0);
536
537 double stec = 0.0;
538 for (unsigned ii = 0; ii < vTec->_layers.size(); ii++) {
539 piercePoint(vTec->_layers[ii]._height, epoch, geocSta.data(), sphEle, sphAzi);
540 double vtec = vtecSingleLayerContribution(vTec->_layers[ii]);
541 stec += vtec * sin(sphEle + _psiPP);
542 }
543 return stec;
544}
545
546double t_iono::vtecSingleLayerContribution(const t_vTecLayer& vTecLayer) {
547
548 double vtec = 0.0;
549 int N = vTecLayer._C.Nrows()-1;
550 int M = vTecLayer._C.Ncols()-1;
551 double fac;
552
553 for (int n = 0; n <= N; n++) {
554 for (int m = 0; m <= min(n, M); m++) {
555 double pnm = associatedLegendreFunction(n, m, sin(_phiPP));
556 double a = double(factorial(n - m));
557 double b = double(factorial(n + m));
558 if (m == 0) {
559 fac = sqrt(2.0 * n + 1);
560 }
561 else {
562 fac = sqrt(2.0 * (2.0 * n + 1) * a / b);
563 }
564 pnm *= fac;
565 double Cnm_mlambda = vTecLayer._C[n][m] * cos(m * _lonS);
566 double Snm_mlambda = vTecLayer._S[n][m] * sin(m * _lonS);
567 vtec += (Snm_mlambda + Cnm_mlambda) * pnm;
568 }
569 }
570
571 if (vtec < 0.0) {
572 return 0.0;
573 }
574
575 return vtec;
576}
577
578void t_iono::piercePoint(double layerHeight, double epoch, const double* geocSta,
579 double sphEle, double sphAzi) {
580
581 double q = (t_CST::rgeoc + geocSta[2]) / (t_CST::rgeoc + layerHeight);
582
583 _psiPP = M_PI/2 - sphEle - asin(q * cos(sphEle));
584
585 _phiPP = asin(sin(geocSta[0]) * cos(_psiPP) + cos(geocSta[0]) * sin(_psiPP) * cos(sphAzi));
586
587 if (( (geocSta[0]*180.0/M_PI > 0) && ( tan(_psiPP) * cos(sphAzi) > tan(M_PI/2 - geocSta[0])) ) ||
588 ( (geocSta[0]*180.0/M_PI < 0) && (-(tan(_psiPP) * cos(sphAzi)) > tan(M_PI/2 + geocSta[0])) )) {
589 _lambdaPP = geocSta[1] + M_PI - asin((sin(_psiPP) * sin(sphAzi) / cos(_phiPP)));
590 } else {
591 _lambdaPP = geocSta[1] + asin((sin(_psiPP) * sin(sphAzi) / cos(_phiPP)));
592 }
593
594 _lonS = fmod((_lambdaPP + (epoch - 50400) * M_PI / 43200), 2*M_PI);
595
596 return;
597}
598
Note: See TracBrowser for help on using the repository browser.