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