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

Last change on this file since 6588 was 6268, checked in by mervart, 10 years ago
File size: 11.7 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#include "bncutils.h"
47
48using namespace BNC_PPP;
49using namespace std;
50
51const double t_astro::RHO_DEG = 180.0 / M_PI;
52const double t_astro::RHO_SEC = 3600.0 * 180.0 / M_PI;
53const double t_astro::MJD_J2000 = 51544.5;
54
55Matrix t_astro::rotX(double Angle) {
56 const double C = cos(Angle);
57 const double S = sin(Angle);
58 Matrix UU(3,3);
59 UU[0][0] = 1.0; UU[0][1] = 0.0; UU[0][2] = 0.0;
60 UU[1][0] = 0.0; UU[1][1] = +C; UU[1][2] = +S;
61 UU[2][0] = 0.0; UU[2][1] = -S; UU[2][2] = +C;
62 return UU;
63}
64
65Matrix t_astro::rotY(double Angle) {
66 const double C = cos(Angle);
67 const double S = sin(Angle);
68 Matrix UU(3,3);
69 UU[0][0] = +C; UU[0][1] = 0.0; UU[0][2] = -S;
70 UU[1][0] = 0.0; UU[1][1] = 1.0; UU[1][2] = 0.0;
71 UU[2][0] = +S; UU[2][1] = 0.0; UU[2][2] = +C;
72 return UU;
73}
74
75Matrix t_astro::rotZ(double Angle) {
76 const double C = cos(Angle);
77 const double S = sin(Angle);
78 Matrix UU(3,3);
79 UU[0][0] = +C; UU[0][1] = +S; UU[0][2] = 0.0;
80 UU[1][0] = -S; UU[1][1] = +C; UU[1][2] = 0.0;
81 UU[2][0] = 0.0; UU[2][1] = 0.0; UU[2][2] = 1.0;
82 return UU;
83}
84
85// Greenwich Mean Sidereal Time
86///////////////////////////////////////////////////////////////////////////
87double t_astro::GMST(double Mjd_UT1) {
88
89 const double Secs = 86400.0;
90
91 double Mjd_0 = floor(Mjd_UT1);
92 double UT1 = Secs*(Mjd_UT1-Mjd_0);
93 double T_0 = (Mjd_0 -MJD_J2000)/36525.0;
94 double T = (Mjd_UT1-MJD_J2000)/36525.0;
95
96 double gmst = 24110.54841 + 8640184.812866*T_0 + 1.002737909350795*UT1
97 + (0.093104-6.2e-6*T)*T*T;
98
99 return 2.0*M_PI*Frac(gmst/Secs);
100}
101
102// Nutation Matrix
103///////////////////////////////////////////////////////////////////////////
104Matrix t_astro::NutMatrix(double Mjd_TT) {
105
106 const double T = (Mjd_TT-MJD_J2000)/36525.0;
107
108 double ls = 2.0*M_PI*Frac(0.993133+ 99.997306*T);
109 double D = 2.0*M_PI*Frac(0.827362+1236.853087*T);
110 double F = 2.0*M_PI*Frac(0.259089+1342.227826*T);
111 double N = 2.0*M_PI*Frac(0.347346- 5.372447*T);
112
113 double dpsi = ( -17.200*sin(N) - 1.319*sin(2*(F-D+N)) - 0.227*sin(2*(F+N))
114 + 0.206*sin(2*N) + 0.143*sin(ls) ) / RHO_SEC;
115 double deps = ( + 9.203*cos(N) + 0.574*cos(2*(F-D+N)) + 0.098*cos(2*(F+N))
116 - 0.090*cos(2*N) ) / RHO_SEC;
117
118 double eps = 0.4090928-2.2696E-4*T;
119
120 return rotX(-eps-deps)*rotZ(-dpsi)*rotX(+eps);
121}
122
123// Precession Matrix
124///////////////////////////////////////////////////////////////////////////
125Matrix t_astro::PrecMatrix(double Mjd_1, double Mjd_2) {
126
127 const double T = (Mjd_1-MJD_J2000)/36525.0;
128 const double dT = (Mjd_2-Mjd_1)/36525.0;
129
130 double zeta = ( (2306.2181+(1.39656-0.000139*T)*T)+
131 ((0.30188-0.000344*T)+0.017998*dT)*dT )*dT/RHO_SEC;
132 double z = zeta + ( (0.79280+0.000411*T)+0.000205*dT)*dT*dT/RHO_SEC;
133 double theta = ( (2004.3109-(0.85330+0.000217*T)*T)-
134 ((0.42665+0.000217*T)+0.041833*dT)*dT )*dT/RHO_SEC;
135
136 return rotZ(-z) * rotY(theta) * rotZ(-zeta);
137}
138
139// Sun's position
140///////////////////////////////////////////////////////////////////////////
141ColumnVector t_astro::Sun(double Mjd_TT) {
142
143 const double eps = 23.43929111/RHO_DEG;
144 const double T = (Mjd_TT-MJD_J2000)/36525.0;
145
146 double M = 2.0*M_PI * Frac ( 0.9931267 + 99.9973583*T);
147 double L = 2.0*M_PI * Frac ( 0.7859444 + M/2.0/M_PI +
148 (6892.0*sin(M)+72.0*sin(2.0*M)) / 1296.0e3);
149 double r = 149.619e9 - 2.499e9*cos(M) - 0.021e9*cos(2*M);
150
151 ColumnVector r_Sun(3);
152 r_Sun << r*cos(L) << r*sin(L) << 0.0; r_Sun = rotX(-eps) * r_Sun;
153
154 return rotZ(GMST(Mjd_TT))
155 * NutMatrix(Mjd_TT)
156 * PrecMatrix(MJD_J2000, Mjd_TT)
157 * r_Sun;
158}
159
160// Moon's position
161///////////////////////////////////////////////////////////////////////////
162ColumnVector t_astro::Moon(double Mjd_TT) {
163
164 const double eps = 23.43929111/RHO_DEG;
165 const double T = (Mjd_TT-MJD_J2000)/36525.0;
166
167 double L_0 = Frac ( 0.606433 + 1336.851344*T );
168 double l = 2.0*M_PI*Frac ( 0.374897 + 1325.552410*T );
169 double lp = 2.0*M_PI*Frac ( 0.993133 + 99.997361*T );
170 double D = 2.0*M_PI*Frac ( 0.827361 + 1236.853086*T );
171 double F = 2.0*M_PI*Frac ( 0.259086 + 1342.227825*T );
172
173 double dL = +22640*sin(l) - 4586*sin(l-2*D) + 2370*sin(2*D) + 769*sin(2*l)
174 -668*sin(lp) - 412*sin(2*F) - 212*sin(2*l-2*D)- 206*sin(l+lp-2*D)
175 +192*sin(l+2*D) - 165*sin(lp-2*D) - 125*sin(D) - 110*sin(l+lp)
176 +148*sin(l-lp) - 55*sin(2*F-2*D);
177
178 double L = 2.0*M_PI * Frac( L_0 + dL/1296.0e3 );
179
180 double S = F + (dL+412*sin(2*F)+541*sin(lp)) / RHO_SEC;
181 double h = F-2*D;
182 double N = -526*sin(h) + 44*sin(l+h) - 31*sin(-l+h) - 23*sin(lp+h)
183 +11*sin(-lp+h) - 25*sin(-2*l+F) + 21*sin(-l+F);
184
185 double B = ( 18520.0*sin(S) + N ) / RHO_SEC;
186
187 double cosB = cos(B);
188
189 double R = 385000e3 - 20905e3*cos(l) - 3699e3*cos(2*D-l) - 2956e3*cos(2*D)
190 -570e3*cos(2*l) + 246e3*cos(2*l-2*D) - 205e3*cos(lp-2*D)
191 -171e3*cos(l+2*D) - 152e3*cos(l+lp-2*D);
192
193 ColumnVector r_Moon(3);
194 r_Moon << R*cos(L)*cosB << R*sin(L)*cosB << R*sin(B);
195 r_Moon = rotX(-eps) * r_Moon;
196
197 return rotZ(GMST(Mjd_TT))
198 * NutMatrix(Mjd_TT)
199 * PrecMatrix(MJD_J2000, Mjd_TT)
200 * r_Moon;
201}
202
203// Tidal Correction
204////////////////////////////////////////////////////////////////////////////
205ColumnVector t_tides::displacement(const bncTime& time, const ColumnVector& xyz) {
206
207 if (time.undef()) {
208 ColumnVector dX(3); dX = 0.0;
209 return dX;
210 }
211
212 double Mjd = time.mjd() + time.daysec() / 86400.0;
213
214 if (Mjd != _lastMjd) {
215 _lastMjd = Mjd;
216 _xSun = t_astro::Sun(Mjd);
217 _rSun = sqrt(DotProduct(_xSun,_xSun));
218 _xSun /= _rSun;
219 _xMoon = t_astro::Moon(Mjd);
220 _rMoon = sqrt(DotProduct(_xMoon,_xMoon));
221 _xMoon /= _rMoon;
222 }
223
224 double rRec = sqrt(DotProduct(xyz, xyz));
225 ColumnVector xyzUnit = xyz / rRec;
226
227 // Love's Numbers
228 // --------------
229 const double H2 = 0.6078;
230 const double L2 = 0.0847;
231
232 // Tidal Displacement
233 // ------------------
234 double scSun = DotProduct(xyzUnit, _xSun);
235 double scMoon = DotProduct(xyzUnit, _xMoon);
236
237 double p2Sun = 3.0 * (H2/2.0-L2) * scSun * scSun - H2/2.0;
238 double p2Moon = 3.0 * (H2/2.0-L2) * scMoon * scMoon - H2/2.0;
239
240 double x2Sun = 3.0 * L2 * scSun;
241 double x2Moon = 3.0 * L2 * scMoon;
242
243 const double gmWGS = 398.6005e12;
244 const double gms = 1.3271250e20;
245 const double gmm = 4.9027890e12;
246
247 double facSun = gms / gmWGS *
248 (rRec * rRec * rRec * rRec) / (_rSun * _rSun * _rSun);
249
250 double facMoon = gmm / gmWGS *
251 (rRec * rRec * rRec * rRec) / (_rMoon * _rMoon * _rMoon);
252
253 ColumnVector dX = facSun * (x2Sun * _xSun + p2Sun * xyzUnit) +
254 facMoon * (x2Moon * _xMoon + p2Moon * xyzUnit);
255
256 return dX;
257}
258
259// Constructor
260///////////////////////////////////////////////////////////////////////////
261t_windUp::t_windUp() {
262 for (unsigned ii = 0; ii <= t_prn::MAXPRN; ii++) {
263 sumWind[ii] = 0.0;
264 lastEtime[ii] = 0.0;
265 }
266}
267
268// Phase Wind-Up Correction
269///////////////////////////////////////////////////////////////////////////
270double t_windUp::value(const bncTime& etime, const ColumnVector& rRec,
271 t_prn prn, const ColumnVector& rSat) {
272
273 if (etime.mjddec() != lastEtime[prn.toInt()]) {
274
275 // Unit Vector GPS Satellite --> Receiver
276 // --------------------------------------
277 ColumnVector rho = rRec - rSat;
278 rho /= rho.norm_Frobenius();
279
280 // GPS Satellite unit Vectors sz, sy, sx
281 // -------------------------------------
282 ColumnVector sz = -rSat / rSat.norm_Frobenius();
283
284 ColumnVector xSun = t_astro::Sun(etime.mjddec());
285 xSun /= xSun.norm_Frobenius();
286
287 ColumnVector sy = crossproduct(sz, xSun);
288 ColumnVector sx = crossproduct(sy, sz);
289
290 // Effective Dipole of the GPS Satellite Antenna
291 // ---------------------------------------------
292 ColumnVector dipSat = sx - rho * DotProduct(rho,sx)
293 - crossproduct(rho, sy);
294
295 // Receiver unit Vectors rx, ry
296 // ----------------------------
297 ColumnVector rx(3);
298 ColumnVector ry(3);
299
300 double recEll[3]; xyz2ell(rRec.data(), recEll) ;
301 double neu[3];
302
303 neu[0] = 1.0;
304 neu[1] = 0.0;
305 neu[2] = 0.0;
306 neu2xyz(recEll, neu, rx.data());
307
308 neu[0] = 0.0;
309 neu[1] = -1.0;
310 neu[2] = 0.0;
311 neu2xyz(recEll, neu, ry.data());
312
313 // Effective Dipole of the Receiver Antenna
314 // ----------------------------------------
315 ColumnVector dipRec = rx - rho * DotProduct(rho,rx)
316 + crossproduct(rho, ry);
317
318 // Resulting Effect
319 // ----------------
320 double alpha = DotProduct(dipSat,dipRec) /
321 (dipSat.norm_Frobenius() * dipRec.norm_Frobenius());
322
323 if (alpha > 1.0) alpha = 1.0;
324 if (alpha < -1.0) alpha = -1.0;
325
326 double dphi = acos(alpha) / 2.0 / M_PI; // in cycles
327
328 if ( DotProduct(rho, crossproduct(dipSat, dipRec)) < 0.0 ) {
329 dphi = -dphi;
330 }
331
332 if (lastEtime[prn.toInt()] == 0.0) {
333 sumWind[prn.toInt()] = dphi;
334 }
335 else {
336 sumWind[prn.toInt()] = nint(sumWind[prn.toInt()] - dphi) + dphi;
337 }
338
339 lastEtime[prn.toInt()] = etime.mjddec();
340 }
341
342 return sumWind[prn.toInt()];
343}
344
345// Tropospheric Model (Saastamoinen)
346////////////////////////////////////////////////////////////////////////////
347double t_tropo::delay_saast(const ColumnVector& xyz, double Ele) {
348
349 Tracer tracer("bncModel::delay_saast");
350
351 if (xyz[0] == 0.0 && xyz[1] == 0.0 && xyz[2] == 0.0) {
352 return 0.0;
353 }
354
355 double ell[3];
356 xyz2ell(xyz.data(), ell);
357 double height = ell[2];
358
359 double pp = 1013.25 * pow(1.0 - 2.26e-5 * height, 5.225);
360 double TT = 18.0 - height * 0.0065 + 273.15;
361 double hh = 50.0 * exp(-6.396e-4 * height);
362 double ee = hh / 100.0 * exp(-37.2465 + 0.213166*TT - 0.000256908*TT*TT);
363
364 double h_km = height / 1000.0;
365
366 if (h_km < 0.0) h_km = 0.0;
367 if (h_km > 5.0) h_km = 5.0;
368 int ii = int(h_km + 1); if (ii > 5) ii = 5;
369 double href = ii - 1;
370
371 double bCor[6];
372 bCor[0] = 1.156;
373 bCor[1] = 1.006;
374 bCor[2] = 0.874;
375 bCor[3] = 0.757;
376 bCor[4] = 0.654;
377 bCor[5] = 0.563;
378
379 double BB = bCor[ii-1] + (bCor[ii]-bCor[ii-1]) * (h_km - href);
380
381 double zen = M_PI/2.0 - Ele;
382
383 return (0.002277/cos(zen)) * (pp + ((1255.0/TT)+0.05)*ee - BB*(tan(zen)*tan(zen)));
384}
385
Note: See TracBrowser for help on using the repository browser.