source: ntrip/trunk/BNC/src/PPP/pppModel.cpp@ 5805

Last change on this file since 5805 was 5805, checked in by mervart, 10 years ago
File size: 9.1 KB
Line 
1
2#include <cmath>
3
4#include "pppModel.h"
5#include "bncutils.h"
6
7using namespace BNC;
8using namespace std;
9
10double Frac (double x) { return x-floor(x); };
11double Modulo (double x, double y) { return y*Frac(x/y); }
12
13Matrix t_astro::rotX(double Angle) {
14 const double C = cos(Angle);
15 const double S = sin(Angle);
16 Matrix UU(3,3);
17 UU[0][0] = 1.0; UU[0][1] = 0.0; UU[0][2] = 0.0;
18 UU[1][0] = 0.0; UU[1][1] = +C; UU[1][2] = +S;
19 UU[2][0] = 0.0; UU[2][1] = -S; UU[2][2] = +C;
20 return UU;
21}
22
23Matrix t_astro::rotY(double Angle) {
24 const double C = cos(Angle);
25 const double S = sin(Angle);
26 Matrix UU(3,3);
27 UU[0][0] = +C; UU[0][1] = 0.0; UU[0][2] = -S;
28 UU[1][0] = 0.0; UU[1][1] = 1.0; UU[1][2] = 0.0;
29 UU[2][0] = +S; UU[2][1] = 0.0; UU[2][2] = +C;
30 return UU;
31}
32
33Matrix t_astro::rotZ(double Angle) {
34 const double C = cos(Angle);
35 const double S = sin(Angle);
36 Matrix UU(3,3);
37 UU[0][0] = +C; UU[0][1] = +S; UU[0][2] = 0.0;
38 UU[1][0] = -S; UU[1][1] = +C; UU[1][2] = 0.0;
39 UU[2][0] = 0.0; UU[2][1] = 0.0; UU[2][2] = 1.0;
40 return UU;
41}
42
43// Greenwich Mean Sidereal Time
44///////////////////////////////////////////////////////////////////////////
45double t_astro::GMST(double Mjd_UT1) {
46
47 const double Secs = 86400.0;
48
49 double Mjd_0 = floor(Mjd_UT1);
50 double UT1 = Secs*(Mjd_UT1-Mjd_0);
51 double T_0 = (Mjd_0 -MJD_J2000)/36525.0;
52 double T = (Mjd_UT1-MJD_J2000)/36525.0;
53
54 double gmst = 24110.54841 + 8640184.812866*T_0 + 1.002737909350795*UT1
55 + (0.093104-6.2e-6*T)*T*T;
56
57 return 2.0*M_PI*Frac(gmst/Secs);
58}
59
60// Nutation Matrix
61///////////////////////////////////////////////////////////////////////////
62Matrix t_astro::NutMatrix(double Mjd_TT) {
63
64 const double T = (Mjd_TT-MJD_J2000)/36525.0;
65
66 double ls = 2.0*M_PI*Frac(0.993133+ 99.997306*T);
67 double D = 2.0*M_PI*Frac(0.827362+1236.853087*T);
68 double F = 2.0*M_PI*Frac(0.259089+1342.227826*T);
69 double N = 2.0*M_PI*Frac(0.347346- 5.372447*T);
70
71 double dpsi = ( -17.200*sin(N) - 1.319*sin(2*(F-D+N)) - 0.227*sin(2*(F+N))
72 + 0.206*sin(2*N) + 0.143*sin(ls) ) / RHO_SEC;
73 double deps = ( + 9.203*cos(N) + 0.574*cos(2*(F-D+N)) + 0.098*cos(2*(F+N))
74 - 0.090*cos(2*N) ) / RHO_SEC;
75
76 double eps = 0.4090928-2.2696E-4*T;
77
78 return rotX(-eps-deps)*rotZ(-dpsi)*rotX(+eps);
79}
80
81// Precession Matrix
82///////////////////////////////////////////////////////////////////////////
83Matrix t_astro::PrecMatrix(double Mjd_1, double Mjd_2) {
84
85 const double T = (Mjd_1-MJD_J2000)/36525.0;
86 const double dT = (Mjd_2-Mjd_1)/36525.0;
87
88 double zeta = ( (2306.2181+(1.39656-0.000139*T)*T)+
89 ((0.30188-0.000344*T)+0.017998*dT)*dT )*dT/RHO_SEC;
90 double z = zeta + ( (0.79280+0.000411*T)+0.000205*dT)*dT*dT/RHO_SEC;
91 double theta = ( (2004.3109-(0.85330+0.000217*T)*T)-
92 ((0.42665+0.000217*T)+0.041833*dT)*dT )*dT/RHO_SEC;
93
94 return rotZ(-z) * rotY(theta) * rotZ(-zeta);
95}
96
97// Sun's position
98///////////////////////////////////////////////////////////////////////////
99ColumnVector t_astro::Sun(double Mjd_TT) {
100
101 const double eps = 23.43929111/RHO_DEG;
102 const double T = (Mjd_TT-MJD_J2000)/36525.0;
103
104 double M = 2.0*M_PI * Frac ( 0.9931267 + 99.9973583*T);
105 double L = 2.0*M_PI * Frac ( 0.7859444 + M/2.0/M_PI +
106 (6892.0*sin(M)+72.0*sin(2.0*M)) / 1296.0e3);
107 double r = 149.619e9 - 2.499e9*cos(M) - 0.021e9*cos(2*M);
108
109 ColumnVector r_Sun(3);
110 r_Sun << r*cos(L) << r*sin(L) << 0.0; r_Sun = rotX(-eps) * r_Sun;
111
112 return rotZ(GMST(Mjd_TT))
113 * NutMatrix(Mjd_TT)
114 * PrecMatrix(MJD_J2000, Mjd_TT)
115 * r_Sun;
116}
117
118// Moon's position
119///////////////////////////////////////////////////////////////////////////
120ColumnVector t_astro::Moon(double Mjd_TT) {
121
122 const double eps = 23.43929111/RHO_DEG;
123 const double T = (Mjd_TT-MJD_J2000)/36525.0;
124
125 double L_0 = Frac ( 0.606433 + 1336.851344*T );
126 double l = 2.0*M_PI*Frac ( 0.374897 + 1325.552410*T );
127 double lp = 2.0*M_PI*Frac ( 0.993133 + 99.997361*T );
128 double D = 2.0*M_PI*Frac ( 0.827361 + 1236.853086*T );
129 double F = 2.0*M_PI*Frac ( 0.259086 + 1342.227825*T );
130
131 double dL = +22640*sin(l) - 4586*sin(l-2*D) + 2370*sin(2*D) + 769*sin(2*l)
132 -668*sin(lp) - 412*sin(2*F) - 212*sin(2*l-2*D)- 206*sin(l+lp-2*D)
133 +192*sin(l+2*D) - 165*sin(lp-2*D) - 125*sin(D) - 110*sin(l+lp)
134 +148*sin(l-lp) - 55*sin(2*F-2*D);
135
136 double L = 2.0*M_PI * Frac( L_0 + dL/1296.0e3 );
137
138 double S = F + (dL+412*sin(2*F)+541*sin(lp)) / RHO_SEC;
139 double h = F-2*D;
140 double N = -526*sin(h) + 44*sin(l+h) - 31*sin(-l+h) - 23*sin(lp+h)
141 +11*sin(-lp+h) - 25*sin(-2*l+F) + 21*sin(-l+F);
142
143 double B = ( 18520.0*sin(S) + N ) / RHO_SEC;
144
145 double cosB = cos(B);
146
147 double R = 385000e3 - 20905e3*cos(l) - 3699e3*cos(2*D-l) - 2956e3*cos(2*D)
148 -570e3*cos(2*l) + 246e3*cos(2*l-2*D) - 205e3*cos(lp-2*D)
149 -171e3*cos(l+2*D) - 152e3*cos(l+lp-2*D);
150
151 ColumnVector r_Moon(3);
152 r_Moon << R*cos(L)*cosB << R*sin(L)*cosB << R*sin(B);
153 r_Moon = rotX(-eps) * r_Moon;
154
155 return rotZ(GMST(Mjd_TT))
156 * NutMatrix(Mjd_TT)
157 * PrecMatrix(MJD_J2000, Mjd_TT)
158 * r_Moon;
159}
160
161// Tidal Correction
162////////////////////////////////////////////////////////////////////////////
163ColumnVector t_tides::displacement(const bncTime& time, const ColumnVector& xyz) {
164
165 double Mjd = time.mjd() + time.daysec() / 86400.0;
166
167 if (Mjd != _lastMjd) {
168 _lastMjd = Mjd;
169 _xSun = t_astro::Sun(Mjd);
170 _rSun = sqrt(DotProduct(_xSun,_xSun));
171 _xSun /= _rSun;
172 _xMoon = t_astro::Moon(Mjd);
173 _rMoon = sqrt(DotProduct(_xMoon,_xMoon));
174 _xMoon /= _rMoon;
175 }
176
177 double rRec = sqrt(DotProduct(xyz, xyz));
178 ColumnVector xyzUnit = xyz / rRec;
179
180 // Love's Numbers
181 // --------------
182 const double H2 = 0.6078;
183 const double L2 = 0.0847;
184
185 // Tidal Displacement
186 // ------------------
187 double scSun = DotProduct(xyzUnit, _xSun);
188 double scMoon = DotProduct(xyzUnit, _xMoon);
189
190 double p2Sun = 3.0 * (H2/2.0-L2) * scSun * scSun - H2/2.0;
191 double p2Moon = 3.0 * (H2/2.0-L2) * scMoon * scMoon - H2/2.0;
192
193 double x2Sun = 3.0 * L2 * scSun;
194 double x2Moon = 3.0 * L2 * scMoon;
195
196 const double gmWGS = 398.6005e12;
197 const double gms = 1.3271250e20;
198 const double gmm = 4.9027890e12;
199
200 double facSun = gms / gmWGS *
201 (rRec * rRec * rRec * rRec) / (_rSun * _rSun * _rSun);
202
203 double facMoon = gmm / gmWGS *
204 (rRec * rRec * rRec * rRec) / (_rMoon * _rMoon * _rMoon);
205
206 ColumnVector dX = facSun * (x2Sun * _xSun + p2Sun * xyzUnit) +
207 facMoon * (x2Moon * _xMoon + p2Moon * xyzUnit);
208
209 return dX;
210}
211
212// Constructor
213///////////////////////////////////////////////////////////////////////////
214t_windUp::t_windUp() {
215 for (unsigned ii = 0; ii <= t_prn::MAXPRN; ii++) {
216 sumWind[ii] = 0.0;
217 lastEtime[ii] = 0.0;
218 }
219}
220
221// Phase Wind-Up Correction
222///////////////////////////////////////////////////////////////////////////
223double t_windUp::value(const bncTime& etime, const ColumnVector& rRec,
224 t_prn prn, const ColumnVector& rSat) {
225
226 if (etime.mjddec() != lastEtime[prn.toInt()]) {
227
228 // Unit Vector GPS Satellite --> Receiver
229 // --------------------------------------
230 ColumnVector rho = rRec - rSat;
231 rho /= rho.norm_Frobenius();
232
233 // GPS Satellite unit Vectors sz, sy, sx
234 // -------------------------------------
235 ColumnVector sz = -rSat / rSat.norm_Frobenius();
236
237 ColumnVector xSun = t_astro::Sun(etime.mjddec());
238 xSun /= xSun.norm_Frobenius();
239
240 ColumnVector sy = crossproduct(sz, xSun);
241 ColumnVector sx = crossproduct(sy, sz);
242
243 // Effective Dipole of the GPS Satellite Antenna
244 // ---------------------------------------------
245 ColumnVector dipSat = sx - rho * DotProduct(rho,sx)
246 - crossproduct(rho, sy);
247
248 // Receiver unit Vectors rx, ry
249 // ----------------------------
250 ColumnVector rx(3);
251 ColumnVector ry(3);
252
253 double recEll[3]; xyz2ell(rRec.data(), recEll) ;
254 double neu[3];
255
256 neu[0] = 1.0;
257 neu[1] = 0.0;
258 neu[2] = 0.0;
259 neu2xyz(recEll, neu, rx.data());
260
261 neu[0] = 0.0;
262 neu[1] = -1.0;
263 neu[2] = 0.0;
264 neu2xyz(recEll, neu, ry.data());
265
266 // Effective Dipole of the Receiver Antenna
267 // ----------------------------------------
268 ColumnVector dipRec = rx - rho * DotProduct(rho,rx)
269 + crossproduct(rho, ry);
270
271 // Resulting Effect
272 // ----------------
273 double alpha = DotProduct(dipSat,dipRec) /
274 (dipSat.norm_Frobenius() * dipRec.norm_Frobenius());
275
276 if (alpha > 1.0) alpha = 1.0;
277 if (alpha < -1.0) alpha = -1.0;
278
279 double dphi = acos(alpha) / 2.0 / M_PI; // in cycles
280
281 if ( DotProduct(rho, crossproduct(dipSat, dipRec)) < 0.0 ) {
282 dphi = -dphi;
283 }
284
285 if (lastEtime[prn.toInt()] == 0.0) {
286 sumWind[prn.toInt()] = dphi;
287 }
288 else {
289 sumWind[prn.toInt()] = nint(sumWind[prn.toInt()] - dphi) + dphi;
290 }
291
292 lastEtime[prn.toInt()] = etime.mjddec();
293 }
294
295 return sumWind[prn.toInt()];
296}
Note: See TracBrowser for help on using the repository browser.