Changeset 8905 in ntrip for trunk/BNC/src/pppModel.cpp


Ignore:
Timestamp:
Mar 18, 2020, 11:13:50 AM (4 years ago)
Author:
stuerze
Message:

some developments regarding PPP, not completed!

File:
1 edited

Legend:

Unmodified
Added
Removed

  • trunk/BNC/src/pppModel.cpp

    r8774 r8905  
    1 
    21// Part of BNC, a utility for retrieving decoding and
    32// converting GNSS data streams from NTRIP broadcasters.
    … …  
    4039 * -----------------------------------------------------------------------*/
    4140
    42 
    4341#include <cmath>
    4442
    … …  
    4846using namespace std;
    4947
    50 const double t_astro::RHO_DEG   = 180.0 / M_PI;
    51 const double t_astro::RHO_SEC   = 3600.0 * 180.0 / M_PI;
    52 const double t_astro::MJD_J2000 = 51544.5;
     48
    5349
    5450Matrix t_astro::rotX(double Angle) {
    5551  const double C = cos(Angle);
    5652  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;
     53  Matrix UU(3, 3);
     54  UU[0][0] = 1.0;
     55  UU[0][1] = 0.0;
     56  UU[0][2] = 0.0;
     57  UU[1][0] = 0.0;
     58  UU[1][1] = +C;
     59  UU[1][2] = +S;
     60  UU[2][0] = 0.0;
     61  UU[2][1] = -S;
     62  UU[2][2] = +C;
    6163  return UU;
    6264}
    … …  
    6567  const double C = cos(Angle);
    6668  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;
     69  Matrix UU(3, 3);
     70  UU[0][0] = +C;
     71  UU[0][1] = 0.0;
     72  UU[0][2] = -S;
     73  UU[1][0] = 0.0;
     74  UU[1][1] = 1.0;
     75  UU[1][2] = 0.0;
     76  UU[2][0] = +S;
     77  UU[2][1] = 0.0;
     78  UU[2][2] = +C;
    7179  return UU;
    7280}
    … …  
    7583  const double C = cos(Angle);
    7684  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;
     85  Matrix UU(3, 3);
     86  UU[0][0] = +C;
     87  UU[0][1] = +S;
     88  UU[0][2] = 0.0;
     89  UU[1][0] = -S;
     90  UU[1][1] = +C;
     91  UU[1][2] = 0.0;
     92  UU[2][0] = 0.0;
     93  UU[2][1] = 0.0;
     94  UU[2][2] = 1.0;
    8195  return UU;
    8296}
    … …  
    89103
    90104  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);
     105  double UT1 = Secs * (Mjd_UT1 - Mjd_0);
     106  double T_0 = (Mjd_0 - MJD_J2000) / 36525.0;
     107  double T = (Mjd_UT1 - MJD_J2000) / 36525.0;
     108
     109  double gmst = 24110.54841 + 8640184.812866 * T_0 + 1.002737909350795 * UT1
     110      + (0.093104 - 6.2e-6 * T) * T * T;
     111
     112  return 2.0 * M_PI * Frac(gmst / Secs);
    99113}
    100114
    … …  
    103117Matrix t_astro::NutMatrix(double Mjd_TT) {
    104118
    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);
     119  const double T = (Mjd_TT - MJD_J2000) / 36525.0;
     120
     121  double ls = 2.0 * M_PI * Frac(0.993133 + 99.997306 * T);
     122  double D = 2.0 * M_PI * Frac(0.827362 + 1236.853087 * T);
     123  double F = 2.0 * M_PI * Frac(0.259089 + 1342.227826 * T);
     124  double N = 2.0 * M_PI * Frac(0.347346 - 5.372447 * T);
     125
     126  double dpsi = (-17.200 * sin(N) - 1.319 * sin(2 * (F - D + N))
     127      - 0.227 * sin(2 * (F + N))
     128      + 0.206 * sin(2 * N) + 0.143 * sin(ls)) / RHO_SEC;
     129  double deps = (+9.203 * cos(N) + 0.574 * cos(2 * (F - D + N))
     130      + 0.098 * cos(2 * (F + N))
     131      - 0.090 * cos(2 * N)) / RHO_SEC;
     132
     133  double eps = 0.4090928 - 2.2696E-4 * T;
     134
     135  return rotX(-eps - deps) * rotZ(-dpsi) * rotX(+eps);
    120136}
    121137
    … …  
    124140Matrix t_astro::PrecMatrix(double Mjd_1, double Mjd_2) {
    125141
    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;
     142  const double T = (Mjd_1 - MJD_J2000) / 36525.0;
     143  const double dT = (Mjd_2 - Mjd_1) / 36525.0;
     144
     145  double zeta = ((2306.2181 + (1.39656 - 0.000139 * T) * T) +
     146      ((0.30188 - 0.000344 * T) + 0.017998 * dT) * dT) * dT / RHO_SEC;
     147  double z = zeta
     148      + ((0.79280 + 0.000411 * T) + 0.000205 * dT) * dT * dT / RHO_SEC;
     149  double theta = ((2004.3109 - (0.85330 + 0.000217 * T) * T) -
     150      ((0.42665 + 0.000217 * T) + 0.041833 * dT) * dT) * dT / RHO_SEC;
    134151
    135152  return rotZ(-z) * rotY(theta) * rotZ(-zeta);
    … …  
    140157ColumnVector t_astro::Sun(double Mjd_TT) {
    141158
    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);
     159  const double eps = 23.43929111 / RHO_DEG;
     160  const double T = (Mjd_TT - MJD_J2000) / 36525.0;
     161
     162  double M = 2.0 * M_PI * Frac(0.9931267 + 99.9973583 * T);
     163  double L = 2.0 * M_PI * Frac(0.7859444 + M / 2.0 / M_PI +
     164      (6892.0 * sin(M) + 72.0 * sin(2.0 * M)) / 1296.0e3);
     165  double r = 149.619e9 - 2.499e9 * cos(M) - 0.021e9 * cos(2 * M);
    149166
    150167  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;
     168  r_Sun << r * cos(L) << r * sin(L) << 0.0;
     169  r_Sun = rotX(-eps) * r_Sun;
     170
     171  return rotZ(GMST(Mjd_TT))
     172      * NutMatrix(Mjd_TT)
     173      * PrecMatrix(MJD_J2000, Mjd_TT)
     174      * r_Sun;
    157175}
    158176
    … …  
    161179ColumnVector t_astro::Moon(double Mjd_TT) {
    162180
    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;
     181  const double eps = 23.43929111 / RHO_DEG;
     182  const double T = (Mjd_TT - MJD_J2000) / 36525.0;
     183
     184  double L_0 = Frac(0.606433 + 1336.851344 * T);
     185  double l = 2.0 * M_PI * Frac(0.374897 + 1325.552410 * T);
     186  double lp = 2.0 * M_PI * Frac(0.993133 + 99.997361 * T);
     187  double D = 2.0 * M_PI * Frac(0.827361 + 1236.853086 * T);
     188  double F = 2.0 * M_PI * Frac(0.259086 + 1342.227825 * T);
     189
     190  double dL = +22640 * sin(l) - 4586 * sin(l - 2 * D) + 2370 * sin(2 * D)
     191      + 769 * sin(2 * l)
     192      - 668 * sin(lp) - 412 * sin(2 * F) - 212 * sin(2 * l - 2 * D)
     193      - 206 * sin(l + lp - 2 * D)
     194      + 192 * sin(l + 2 * D) - 165 * sin(lp - 2 * D) - 125 * sin(D)
     195      - 110 * sin(l + lp)
     196      + 148 * sin(l - lp) - 55 * sin(2 * F - 2 * D);
     197
     198  double L = 2.0 * M_PI * Frac(L_0 + dL / 1296.0e3);
     199
     200  double S = F + (dL + 412 * sin(2 * F) + 541 * sin(lp)) / RHO_SEC;
     201  double h = F - 2 * D;
     202  double N = -526 * sin(h) + 44 * sin(l + h) - 31 * sin(-l + h)
     203      - 23 * sin(lp + h)
     204      + 11 * sin(-lp + h) - 25 * sin(-2 * l + F) + 21 * sin(-l + F);
     205
     206  double B = (18520.0 * sin(S) + N) / RHO_SEC;
    185207
    186208  double cosB = cos(B);
    187209
    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);
     210  double R = 385000e3 - 20905e3 * cos(l) - 3699e3 * cos(2 * D - l)
     211      - 2956e3 * cos(2 * D)
     212      - 570e3 * cos(2 * l) + 246e3 * cos(2 * l - 2 * D)
     213      - 205e3 * cos(lp - 2 * D)
     214      - 171e3 * cos(l + 2 * D) - 152e3 * cos(l + lp - 2 * D);
    191215
    192216  ColumnVector r_Moon(3);
    193   r_Moon << R*cos(L)*cosB << R*sin(L)*cosB << R*sin(B);
     217  r_Moon << R * cos(L) * cosB << R * sin(L) * cosB << R * sin(B);
    194218  r_Moon = rotX(-eps) * r_Moon;
    195219
    196   return    rotZ(GMST(Mjd_TT))
    197           * NutMatrix(Mjd_TT)
    198           * PrecMatrix(MJD_J2000, Mjd_TT)
    199           * r_Moon;
     220  return rotZ(GMST(Mjd_TT))
     221      * NutMatrix(Mjd_TT)
     222      * PrecMatrix(MJD_J2000, Mjd_TT)
     223      * r_Moon;
    200224}
    201225
    202226// Tidal Correction
    203227////////////////////////////////////////////////////////////////////////////
    204 ColumnVector t_tides::displacement(const bncTime& time, const ColumnVector& xyz) {
     228ColumnVector t_tides::earth(const bncTime& time, const ColumnVector& xyz) {
    205229
    206230  if (time.undef()) {
    207     ColumnVector dX(3); dX = 0.0;
     231    ColumnVector dX(3);
     232    dX = 0.0;
    208233    return dX;
    209234  }
    … …  
    214239    _lastMjd = Mjd;
    215240    _xSun = t_astro::Sun(Mjd);
    216     _rSun = sqrt(DotProduct(_xSun,_xSun));
     241    _rSun = sqrt(DotProduct(_xSun, _xSun));
    217242    _xSun /= _rSun;
    218243    _xMoon = t_astro::Moon(Mjd);
    219     _rMoon = sqrt(DotProduct(_xMoon,_xMoon));
     244    _rMoon = sqrt(DotProduct(_xMoon, _xMoon));
    220245    _xMoon /= _rMoon;
    221246  }
    222247
    223   double       rRec    = sqrt(DotProduct(xyz, xyz));
     248  double rRec = sqrt(DotProduct(xyz, xyz));
    224249  ColumnVector xyzUnit = xyz / rRec;
    225250
    … …  
    231256  // Tidal Displacement
    232257  // ------------------
    233   double scSun  = DotProduct(xyzUnit, _xSun);
     258  double scSun = DotProduct(xyzUnit, _xSun);
    234259  double scMoon = DotProduct(xyzUnit, _xMoon);
    235260
    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;
     261  double p2Sun = 3.0 * (H2 / 2.0 - L2) * scSun * scSun - H2 / 2.0;
     262  double p2Moon = 3.0 * (H2 / 2.0 - L2) * scMoon * scMoon - H2 / 2.0;
     263
     264  double x2Sun = 3.0 * L2 * scSun;
    240265  double x2Moon = 3.0 * L2 * scMoon;
    241266
    242267  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);
     268  const double gms = 1.3271250e20;
     269  const double gmm = 4.9027890e12;
     270
     271  double facSun = gms / gmWGS *
     272      (rRec * rRec * rRec * rRec) / (_rSun * _rSun * _rSun);
    248273
    249274  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);
     275      (rRec * rRec * rRec * rRec) / (_rMoon * _rMoon * _rMoon);
     276
     277  ColumnVector dX = facSun * (x2Sun * _xSun + p2Sun * xyzUnit)
     278                  + facMoon * (x2Moon * _xMoon + p2Moon * xyzUnit);
    254279
    255280  return dX;
    … …  
    258283// Constructor
    259284///////////////////////////////////////////////////////////////////////////
    260 t_loading::t_loading(const QString& fileName) {
    261 
     285t_tides::t_tides() {
     286  _lastMjd = 0.0;
     287  _rSun = 0.0;
     288  _rMoon = 0.0;
    262289  newBlqData = 0;
    263   readFile(fileName);
    264   printAll();
    265 }
    266 
    267 t_loading::~t_loading() {
     290}
     291
     292t_tides::~t_tides() {
    268293
    269294  if (newBlqData) {
    … …  
    278303}
    279304
    280 t_irc t_loading::readFile(const QString& fileName) {
     305t_irc t_tides::readBlqFile(const char* fileName) {
    281306  QFile inFile(fileName);
    282307  inFile.open(QIODevice::ReadOnly | QIODevice::Text);
    … …  
    285310  QString site = QString();
    286311
    287   while ( !in.atEnd() ) {
     312  while (!in.atEnd()) {
    288313
    289314    QString line = in.readLine();
    290315
    291316    // skip empty lines and comments
    292     if (line.indexOf("$$") != -1 || line.size() == 0) {
     317    if (line.indexOf("$$") != -1) {
    293318      continue;
    294319    }
    295 
    296320    line = line.trimmed();
    297321    QTextStream inLine(line.toLatin1(), QIODevice::ReadOnly);
    298 
    299322    switch (row) {
    300323      case 0:
    301324        site = line;
    302325        site = site.toUpper();
    303         if (!newBlqData) {
    304           newBlqData = new t_blqData;
    305           newBlqData->amplitudes.ReSize(3,11);
    306           newBlqData->phases.ReSize(3,11);
     326        newBlqData = new t_blqData;
     327        newBlqData->amplitudes.ReSize(3, 11);
     328        newBlqData->phases.ReSize(3, 11);
     329        break;
     330      case 1:
     331      case 2:
     332      case 3:
     333        for (int ii = 0; ii < 11; ii++) {
     334          inLine >> newBlqData->amplitudes[row - 1][ii];
    307335        }
    308336        break;
    309       case 1: case 2: case 3:
     337      case 4:
     338      case 5:
    310339        for (int ii = 0; ii < 11; ii++) {
    311           inLine >> newBlqData->amplitudes[row-1][ii];
     340          inLine >> newBlqData->phases[row - 4][ii];
    312341        }
    313342        break;
    314       case 4: case 5: case 6:
     343      case 6:
    315344        for (int ii = 0; ii < 11; ii++) {
    316           inLine >> newBlqData->phases[row-4][ii];
     345          inLine >> newBlqData->phases[row - 4][ii];
    317346        }
    318         break;
    319       case 7:
    320347        if (newBlqData && !site.isEmpty()) {
    321348          blqMap[site] = newBlqData;
    322349          site = QString();
    323           row = -1;
    324350          newBlqData = 0;
    325351        }
     352        row = -1;
    326353        break;
    327354    }
    … …  
    332359}
    333360
     361ColumnVector t_tides::ocean(const bncTime& time,  const ColumnVector& xyz,
     362    const std::string& station) {
     363  ColumnVector dX(3); dX = 0.0;
     364  if (time.undef()) {
     365    return dX;
     366  }
     367  QString stationQ = station.c_str();
     368  if (blqMap.find(stationQ) == blqMap.end()) {
     369    return dX;
     370  }
     371  t_blqData* blqSet = blqMap[stationQ];  //printBlqSet(station, blqSet);
     372
     373  // angular argument: see arg2.f from IERS Conventions software collection
     374  double speed[11] = {1.40519e-4, 1.45444e-4, 1.3788e-4, 1.45842e-4, 7.2921e-5,
     375                      6.7598e-5,  7.2523e-5,  6.4959e-5, 5.3234e-6,  2.6392e-6, 3.982e-7};
     376
     377  double angfac[4][11];
     378  angfac[0][0] = 2.0;
     379  angfac[1][0] =-2.0;
     380  angfac[2][0] = 0.0;
     381  angfac[3][0] = 0.0;
     382
     383  angfac[0][1] = 0.0;
     384  angfac[1][1] = 0.0;
     385  angfac[2][1] = 0.0;
     386  angfac[3][1] = 0.0;
     387
     388  angfac[0][2] = 2.0;
     389  angfac[1][2] =-3.0;
     390  angfac[2][2] = 1.0;
     391  angfac[3][2] = 0.0;
     392
     393  angfac[0][3] = 2.0;
     394  angfac[1][3] = 0.0;
     395  angfac[2][3] = 0.0;
     396  angfac[3][3] = 0.0;
     397
     398  angfac[0][4] = 1.0;
     399  angfac[1][4] = 0.0;
     400  angfac[2][4] = 0.0;
     401  angfac[3][4] = .25;
     402
     403  angfac[0][5] = 1.0;
     404  angfac[1][5] =-2.0;
     405  angfac[2][5] = 0.0;
     406  angfac[3][5] =-.25;
     407
     408  angfac[0][6] =-1.0;
     409  angfac[1][6] = 0.0;
     410  angfac[2][6] = 0.0;
     411  angfac[3][6] =-.25;
     412
     413  angfac[0][7] = 1.0;
     414  angfac[1][7] =-3.0;
     415  angfac[2][7] = 1.0;
     416  angfac[3][7] =-.25;
     417
     418  angfac[0][8] = 0.0;
     419  angfac[1][8] = 2.0;
     420  angfac[2][8] = 0.0;
     421  angfac[3][8] = 0.0;
     422
     423  angfac[0][9] = 0.0;
     424  angfac[1][9] = 1.0;
     425  angfac[2][9] =-1.0;
     426  angfac[3][9] = 0.0;
     427
     428  angfac[0][10] = 2.0;
     429  angfac[1][10] = 0.0;
     430  angfac[2][10] = 0.0;
     431  angfac[3][10] = 0.0;
     432
     433  double twopi = 6.283185307179586476925287e0;
     434  double dtr = 0.0174532925199;
     435
     436  //  fractional part of the day in seconds
     437  unsigned int year, month, day;
     438  time.civil_date(year, month, day);
     439  int iyear = year - 2000;
     440  QDateTime datTim = QDateTime::fromString(QString::fromStdString(time.datestr()), Qt::ISODate);
     441  int doy = datTim.date().dayOfYear();
     442  double fday = time.daysec();
     443  int   icapd = doy + 365 * (iyear - 75) + ((iyear - 73) / 4);
     444  double capt = (icapd * 1.000000035 + 27392.500528) / 36525.0;
     445
     446  // mean longitude of the sun at the beginning of the day
     447  double h0 = (279.69668e0 + (36000.768930485e0 + 3.03e-4 * capt) * capt) * dtr;
     448
     449  // mean longitude of moon at the beginning of the day
     450  double s0 = (((1.9e-6 * capt - .001133e0) * capt + 481267.88314137e0) * capt + 270.434358e0) * dtr;
     451
     452  // mean longitude of lunar perigee at the beginning of the day
     453  double p0 =  (((-1.2e-5 * capt - .010325e0) * capt + 4069.0340329577e0) * capt + 334.329653e0) * dtr;
     454
     455  // tidal angle arguments
     456  double angle[11];
     457  for (int k = 0; k < 11; ++k) {
     458    angle[k] = speed[k] * fday
     459             + angfac[0][k] * h0
     460             + angfac[1][k] * s0
     461             + angfac[2][k] * p0
     462             + angfac[3][k] * twopi;
     463    angle[k] = fmod(angle[k], twopi);
     464    if (angle[k] < 0.0) {
     465      angle[k] += twopi;
     466    }
     467  }
     468
     469  // displacement by 11 constituents
     470  ColumnVector rwsSta(3); rwsSta = 0.0; // radial, west, south
     471  for (int rr = 0; rr < 3; rr++) {
     472    for (int cc = 0; cc < 11; cc++) {
     473      rwsSta[rr] += blqSet->amplitudes[rr][cc] * cos((angle[cc] - (blqSet->phases[rr][cc]/RHO_DEG)));
     474    }
     475  }
     476
     477  // neu2xyz
     478  ColumnVector dneu(3); // neu
     479  dneu[0] = -rwsSta[2];
     480  dneu[1] = -rwsSta[1];
     481  dneu[2] =  rwsSta[0];
     482  double recEll[3]; xyz2ell(xyz.data(), recEll) ;
     483  neu2xyz(recEll, dneu.data(), dX.data());
     484
     485  return dX;
     486}
     487
    334488// Print
    335489////////////////////////////////////////////////////////////////////////////
    336 void t_loading::printAll() const {
     490void t_tides::printAllBlqSets() const {
    337491
    338492  QMapIterator<QString, t_blqData*> it(blqMap);
    … …  
    341495    t_blqData* blq = it.value();
    342496    QString site = it.key();
    343     cout << site.toStdString().c_str() << "\n";
     497    cout << site.toStdString().c_str() << "\n===============\n";
    344498    for (int rr = 0; rr < 3; rr++) {
    345499      for (int cc = 0; cc < 11; cc++) {
    … …  
    357511}
    358512
     513// Print
     514////////////////////////////////////////////////////////////////////////////
     515void t_tides::printBlqSet(const std::string& station, t_blqData* blq) {
     516  cout << station << endl;
     517  for (int rr = 0; rr < 3; rr++) {
     518    for (int cc = 0; cc < 11; cc++) {
     519      cout << blq->amplitudes[rr][cc] << " ";
     520    }
     521    cout << endl;
     522  }
     523  for (int rr = 0; rr < 3; rr++) {
     524    for (int cc = 0; cc < 11; cc++) {
     525      cout << blq->phases[rr][cc] << " ";
     526    }
     527    cout << endl;
     528  }
     529}
     530
    359531// Constructor
    360532///////////////////////////////////////////////////////////////////////////
    361533t_windUp::t_windUp() {
    362534  for (unsigned ii = 0; ii <= t_prn::MAXPRN; ii++) {
    363     sumWind[ii]   = 0.0;
     535    sumWind[ii] = 0.0;
    364536    lastEtime[ii] = 0.0;
    365537  }
    … …  
    369541///////////////////////////////////////////////////////////////////////////
    370542double t_windUp::value(const bncTime& etime, const ColumnVector& rRec,
    371                        t_prn prn, const ColumnVector& rSat, bool ssr,
    372                        double yaw, const ColumnVector& vSat) {
     543    t_prn prn, const ColumnVector& rSat, bool ssr,
     544    double yaw, const ColumnVector& vSat) {
    373545
    374546  if (etime.mjddec() != lastEtime[prn.toInt()]) {
    … …  
    377549    // --------------------------------------
    378550    ColumnVector rho = rRec - rSat;
    379     rho /= rho.norm_Frobenius();
     551    rho /= rho.NormFrobenius();
    380552
    381553    // GPS Satellite unit Vectors sz, sy, sx
    … …  
    392564      sHlp = vSat + crossproduct(Omega, rSat);
    393565    }
    394     sHlp /= sHlp.norm_Frobenius();
    395 
    396     ColumnVector sz = -rSat / rSat.norm_Frobenius();
     566    sHlp /= sHlp.NormFrobenius();
     567
     568    ColumnVector sz = -rSat / rSat.NormFrobenius();
    397569    ColumnVector sy = crossproduct(sz, sHlp);
    398570    ColumnVector sx = crossproduct(sy, sz);
    399571
    400     // Yaw consideration
    401     sx = t_astro::rotZ(yaw) * sx;
    402 
     572    if (ssr) {
     573      // Yaw angle consideration
     574      Matrix SXYZ(3, 3);
     575      SXYZ.Column(1) = sx;
     576      SXYZ.Column(2) = sy;
     577      SXYZ.Column(3) = sz;
     578      SXYZ = DotProduct(t_astro::rotZ(yaw), SXYZ);
     579      sx = SXYZ.Column(1);
     580      sy = SXYZ.Column(2);
     581      sz = SXYZ.Column(3);
     582    }
    403583    // Effective Dipole of the GPS Satellite Antenna
    404584    // ---------------------------------------------
    405     ColumnVector dipSat = sx - rho * DotProduct(rho,sx) - crossproduct(rho, sy);
     585    ColumnVector dipSat = sx - rho * DotProduct(rho, sx)
     586        - crossproduct(rho, sy);
    406587
    407588    // Receiver unit Vectors rx, ry
    … …  
    409590    ColumnVector rx(3);
    410591    ColumnVector ry(3);
    411     double recEll[3]; xyz2ell(rRec.data(), recEll) ;
     592    double recEll[3];
     593    xyz2ell(rRec.data(), recEll);
    412594    double neu[3];
    413595
    … …  
    417599    neu2xyz(recEll, neu, rx.data());
    418600
    419     neu[0] =  0.0;
     601    neu[0] = 0.0;
    420602    neu[1] = -1.0;
    421     neu[2] =  0.0;
     603    neu[2] = 0.0;
    422604    neu2xyz(recEll, neu, ry.data());
    423605
    424606    // Effective Dipole of the Receiver Antenna
    425607    // ----------------------------------------
    426     ColumnVector dipRec = rx - rho * DotProduct(rho,rx) + crossproduct(rho, ry);
     608    ColumnVector dipRec = rx - rho * DotProduct(rho, rx)
     609        + crossproduct(rho, ry);
    427610
    428611    // Resulting Effect
    429612    // ----------------
    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 */
     613    double alpha = DotProduct(dipSat, dipRec)
     614        / (dipSat.NormFrobenius() * dipRec.NormFrobenius());
     615
     616    if (alpha > 1.0)
     617      alpha = 1.0;
     618    if (alpha < -1.0)
     619      alpha = -1.0;
     620
    436621    double dphi = acos(alpha) / 2.0 / M_PI;  // in cycles
    437622
    438     if ( DotProduct(rho, crossproduct(dipSat, dipRec)) < 0.0 ) {
     623    if (DotProduct(rho, crossproduct(dipSat, dipRec)) < 0.0) {
    439624      dphi = -dphi;
    440625    }
    … …  
    467652  double height = ell[2];
    468653
    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);
     654  double pp = 1013.25 * pow(1.0 - 2.26e-5 * height, 5.225);
     655  double TT = 18.0 - height * 0.0065 + 273.15;
     656  double hh = 50.0 * exp(-6.396e-4 * height);
     657  double ee = hh / 100.0
     658      * exp(-37.2465 + 0.213166 * TT - 0.000256908 * TT * TT);
    473659
    474660  double h_km = height / 1000.0;
    475661
    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;
     662  if (h_km < 0.0)
     663    h_km = 0.0;
     664  if (h_km > 5.0)
     665    h_km = 5.0;
     666  int ii = int(h_km + 1);
     667  if (ii > 5)
     668    ii = 5;
    479669  double href = ii - 1;
    480670
    … …  
    487677  bCor[5] = 0.563;
    488678
    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)));
     679  double BB = bCor[ii - 1] + (bCor[ii] - bCor[ii - 1]) * (h_km - href);
     680
     681  double zen = M_PI / 2.0 - Ele;
     682
     683  return (0.002277 / cos(zen))
     684      * (pp + ((1255.0 / TT) + 0.05) * ee - BB * (tan(zen) * tan(zen)));
    494685}
    495686
    … …  
    500691}
    501692
    502 t_iono::~t_iono() {}
     693t_iono::~t_iono() {
     694}
    503695
    504696double t_iono::stec(const t_vTec* vTec, double signalPropagationTime,
    505       const ColumnVector& rSat, const bncTime& epochTime,
    506       const ColumnVector& xyzSta) {
     697    const ColumnVector& rSat, const bncTime& epochTime,
     698    const ColumnVector& xyzSta) {
    507699
    508700  // Latitude, longitude, height are defined with respect to a spherical earth model
    … …  
    524716  // -------------------------------------------------------------
    525717  ColumnVector rhoV = xyzSat - xyzSta;
    526   double rho = rhoV.norm_Frobenius();
     718  double rho = rhoV.NormFrobenius();
    527719  ColumnVector neu(3);
    528720  xyz2neu(geocSta.data(), rhoV.data(), neu.data());
    529   double sphEle = acos( sqrt(neu[0]*neu[0] + neu[1]*neu[1]) / rho );
     721  double sphEle = acos(sqrt(neu[0] * neu[0] + neu[1] * neu[1]) / rho);
    530722  if (neu[2] < 0) {
    531723    sphEle *= -1.0;
    … …  
    537729  double stec = 0.0;
    538730  for (unsigned ii = 0; ii < vTec->_layers.size(); ii++) {
    539     piercePoint(vTec->_layers[ii]._height, epoch, geocSta.data(), sphEle, sphAzi);
     731    piercePoint(vTec->_layers[ii]._height, epoch, geocSta.data(), sphEle,
     732        sphAzi);
    540733    double vtec = vtecSingleLayerContribution(vTec->_layers[ii]);
    541734    stec += vtec * sin(sphEle + _psiPP);
    … …  
    547740
    548741  double vtec = 0.0;
    549   int N = vTecLayer._C.Nrows()-1;
    550   int M = vTecLayer._C.Ncols()-1;
     742  int N = vTecLayer._C.Nrows() - 1;
     743  int M = vTecLayer._C.Ncols() - 1;
    551744  double fac;
    552745
    … …  
    576769}
    577770
    578 void t_iono::piercePoint(double layerHeight, double epoch, const double* geocSta,
     771void t_iono::piercePoint(double layerHeight, double epoch,
     772    const double* geocSta,
    579773    double sphEle, double sphAzi) {
    580774
    581775  double q = (t_CST::rgeoc + geocSta[2]) / (t_CST::rgeoc + layerHeight);
    582776
    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);
     777  _psiPP = M_PI / 2 - sphEle - asin(q * cos(sphEle));
     778
     779  _phiPP = asin(
     780      sin(geocSta[0]) * cos(_psiPP)
     781          + cos(geocSta[0]) * sin(_psiPP) * cos(sphAzi));
     782
     783  if (((geocSta[0] * 180.0 / M_PI > 0)
     784      && (tan(_psiPP) * cos(sphAzi) > tan(M_PI / 2 - geocSta[0])))
     785      ||
     786      ((geocSta[0] * 180.0 / M_PI < 0)
     787          && (-(tan(_psiPP) * cos(sphAzi)) > tan(M_PI / 2 + geocSta[0])))) {
     788    _lambdaPP = geocSta[1] + M_PI
     789        - asin((sin(_psiPP) * sin(sphAzi) / cos(_phiPP)));
     790  }
     791  else {
     792    _lambdaPP = geocSta[1] + asin((sin(_psiPP) * sin(sphAzi) / cos(_phiPP)));
     793  }
     794
     795  _lonS = fmod((_lambdaPP + (epoch - 50400) * M_PI / 43200), 2 * M_PI);
    595796
    596797  return;
Note: See TracChangeset for help on using the changeset viewer.