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