00001 #pragma ident "$Id: ReferenceFrames.cpp 3140 2012-06-18 15:03:02Z susancummins $"
00002
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include "ReferenceFrames.hpp"
00031 #include <iostream>
00032 #include <string>
00033 #include <vector>
00034 #include <cmath>
00035 #include "StringUtils.hpp"
00036 #include "IERS.hpp"
00037 #include "ASConstant.hpp"
00038
00039
00040 namespace gpstk
00041 {
00042 using namespace std;
00043
00044
00045 SolarSystem ReferenceFrames::solarPlanets;
00046
00047
00048 const double ReferenceFrames::DJ00 = 2451545.0;
00049
00050
00051 const double ReferenceFrames::JD_TO_MJD = 2400000.5;
00052
00053
00054 const double ReferenceFrames::D2PI = 6.283185307179586476925287;
00055
00056
00057 const double ReferenceFrames::DJC = 36525.0;
00058
00059
00060 const double ReferenceFrames::DAS2R = 4.848136811095359935899141e-6;
00061
00062
00063 const double ReferenceFrames::DS2R = 7.272205216643039903848712e-5;
00064
00065
00066 const double ReferenceFrames::TURNAS = 1296000.0;
00067
00068
00069
00070
00071
00072
00073
00074
00075 Vector<double> ReferenceFrames::getJ2kPosition( const CommonTime& TT,
00076 SolarSystem::Planet entity)
00077 throw(Exception)
00078 {
00079 Vector<double> rvJ2k = getJ2kPosVel(TT,entity);
00080
00081 Vector<double> rJ2k(3,0.0);
00082 for(int i=0; i<3; i++)
00083 {
00084 rJ2k[i] = rvJ2k[i];
00085 }
00086
00087 return rJ2k;
00088 }
00089
00090
00091
00092
00093
00094
00095
00096 Vector<double> ReferenceFrames::getJ2kVelocity( const CommonTime& TT,
00097 SolarSystem::Planet entity)
00098 throw(Exception)
00099 {
00100 Vector<double> rvJ2k = getJ2kPosVel(TT,entity);
00101
00102 Vector<double> vJ2k(3,0.0);
00103 for(int i = 0; i < 3; i++)
00104 {
00105 vJ2k[i] = rvJ2k[i+3];
00106 }
00107
00108 return vJ2k;
00109 }
00110
00111
00112
00113
00114
00115
00116
00117
00118 Vector<double> ReferenceFrames::getJ2kPosVel( const CommonTime& TT,
00119 SolarSystem::Planet entity,
00120 SolarSystem::Planet center)
00121 throw(Exception)
00122 {
00123 Vector<double> rvJ2k(6,0.0);
00124
00125 try
00126 {
00127 double rvState[6] = {0.0};
00128 int ret = solarPlanets.computeState(JD_TO_MJD + static_cast<Epoch>(TT).MJD(),
00129 entity,
00130 center,
00131 rvState);
00132
00133
00134 rvState[3] /= 86400.0;
00135 rvState[4] /= 86400.0;
00136 rvState[5] /= 86400.0;
00137
00138 if(ret == 0)
00139 {
00140 rvJ2k = rvState;
00141 return rvJ2k;
00142 }
00143 else
00144 {
00145 rvJ2k.resize(6,0.0);
00146
00147
00148 InvalidRequest e("Failed to compute, error code: "
00149 +StringUtils::asString(ret)+" with meaning\n"
00150 +"-1 and -2 given time is out of the file \n"
00151 +"-3 and -4 input stream is not open or not valid,"
00152 +" or EOF was found prematurely");
00153
00154 GPSTK_THROW(e);
00155 }
00156 }
00157 catch(Exception& e)
00158 {
00159 GPSTK_RETHROW(e);
00160 }
00161 catch(exception& e)
00162 {
00163 Exception ee(e.what());
00164 GPSTK_THROW(ee);
00165 }
00166 catch(...)
00167 {
00168 Exception e("Unknown error!");
00169 GPSTK_THROW(e);
00170 }
00171
00172 return rvJ2k;
00173
00174 }
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184 Vector<double> ReferenceFrames::getECEFPosition(UTCTime UTC,
00185 SolarSystem::Planet entity,
00186 SolarSystem::Planet center)
00187 throw(Exception)
00188 {
00189 Vector<double> ecefPosVel = getECEFPosVel(UTC, entity, center);
00190
00191 Vector<double> ecefPos(3,0.0);
00192 ecefPos(0) = ecefPosVel(0);
00193 ecefPos(1) = ecefPosVel(1);
00194 ecefPos(2) = ecefPosVel(2);
00195
00196 return ecefPos;
00197 }
00198
00199
00200
00201
00202
00203
00204
00205
00206 Vector<double> ReferenceFrames::getECEFVelocity(UTCTime UTC,
00207 SolarSystem::Planet entity,
00208 SolarSystem::Planet center)
00209 throw(Exception)
00210 {
00211 Vector<double> ecefPosVel = getECEFPosVel(UTC, entity, center);
00212
00213 Vector<double> ecefVel(3,0.0);
00214 ecefVel(0) = ecefPosVel(3);
00215 ecefVel(1) = ecefPosVel(4);
00216 ecefVel(2) = ecefPosVel(5);
00217
00218 return ecefVel;
00219 }
00220
00221
00222
00223
00224
00225
00226
00227
00228 Vector<double> ReferenceFrames::getECEFPosVel( UTCTime UTC,
00229 SolarSystem::Planet entity,
00230 SolarSystem::Planet center)
00231 throw(Exception)
00232 {
00233 Vector<double> j2kPosVel = getJ2kPosVel( UTC.asTT(), entity, center);
00234 Vector<double> ecefPosVel = J2kPosVelToECEF(UTC, j2kPosVel);
00235
00236 return ecefPosVel;
00237 }
00238
00239
00240 void ReferenceFrames::J2kToECEFMatrix(UTCTime UTC,
00241 Matrix<double>& POM,
00242 Matrix<double>& Theta,
00243 Matrix<double>& NP)
00244 throw(Exception)
00245 {
00246
00247 double xp = UTC.xPole() * DAS2R;
00248 double yp = UTC.yPole() * DAS2R;
00249
00250 CommonTime TT = UTC.asTT();
00251 CommonTime UT1 = UTC.asUT1();
00252
00253
00254
00255 Matrix<double> P = iauPmat76(TT);
00256
00257
00258 const double DDP80 = 0.0;
00259 const double DDE80 = 0.0;
00260
00261
00262 double DPSI = 0.0;
00263 double DEPS = 0.0;
00264 nutationAngles(TT, DPSI, DEPS);
00265
00266 DPSI += DDP80;
00267 DEPS += DDE80;
00268
00269
00270 double EPSA = meanObliquity(TT);
00271
00272
00273 Matrix<double> N = iauNmat(EPSA, DPSI , DEPS);
00274
00275
00276 NP = N * P;
00277
00278
00279 double EE = iauEqeq94(TT) + DDP80 * std::cos(EPSA);
00280
00281
00282 double GST = normalizeAngle(iauGmst82(UT1) + EE);
00283
00284 Theta = Rz(GST);
00285
00286
00287 POM = Ry(-xp) * Rx(-yp);
00288
00289
00290
00291 return;
00292
00293 }
00294
00295
00296
00297 Matrix<double> ReferenceFrames::J2kToECEFMatrix(UTCTime UTC)
00298 {
00299 Matrix<double> POM, Theta, NP;
00300 J2kToECEFMatrix(UTC,POM,Theta,NP);
00301
00302 return (POM * Theta * NP);
00303 }
00304
00306 Matrix<double> ReferenceFrames::J2kToTODMatrix(UTCTime UTC)
00307 {
00308 Matrix<double> POM, Theta, NP;
00309 J2kToECEFMatrix(UTC,POM,Theta,NP);
00310
00311 return NP;
00312 }
00313
00314
00315 Vector<double> ReferenceFrames::J2kPosVelToECEF(UTCTime UTC, Vector<double> j2kPosVel)
00316 throw(Exception)
00317 {
00318
00319 Matrix<double> POM, Theta, NP;
00320 J2kToECEFMatrix(UTC,POM,Theta,NP);
00321
00322 const double dera = earthRotationAngleRate1(UTC.mjdTT());
00323
00324
00325 Matrix<double> S(3,3,0.0);
00326 S(0,1) = 1.0; S(1,0) = -1.0;
00327
00328 Matrix<double> dTheta = dera * S * Theta;
00329
00330 Matrix<double> c2t = POM * Theta * NP;
00331 Matrix<double> dc2t = POM * dTheta * NP;
00332
00333 Vector<double> j2kPos(3, 0.0), j2kVel(3, 0.0);
00334 for(int i=0; i<3; i++)
00335 {
00336 j2kPos(i) = j2kPosVel(i);
00337 j2kVel(i) = j2kPosVel(i+3);
00338 }
00339
00340 Vector<double> ecefPos = c2t * j2kPos;
00341 Vector<double> ecefVel = c2t * j2kVel + dc2t * j2kPos;
00342
00343 Vector<double> ecefPosVel(6,0.0);
00344 for(int i=0; i<3; i++)
00345 {
00346 ecefPosVel(i) = ecefPos(i);
00347 ecefPosVel(i+3) = ecefVel(i);
00348 }
00349
00350 return ecefPosVel;
00351
00352 }
00353
00354
00355 Vector<double> ReferenceFrames::ECEFPosVelToJ2k(UTCTime UTC, Vector<double> ecefPosVel)
00356 throw(Exception)
00357 {
00358 Matrix<double> POM, Theta, NP;
00359 J2kToECEFMatrix(UTC,POM,Theta,NP);
00360
00361 const double dera = earthRotationAngleRate1(UTC.mjdTT());
00362
00363
00364 Matrix<double> S(3,3,0.0);
00365 S(0,1) = 1.0; S(1,0) = -1.0;
00366
00367 Matrix<double> dTheta = dera * S * Theta;
00368
00369 Matrix<double> c2t = POM * Theta * NP;
00370 Matrix<double> dc2t = POM * dTheta * NP;
00371
00372 Vector<double> ecefPos(3, 0.0), ecefVel(3, 0.0);
00373 for(int i=0; i<3; i++)
00374 {
00375 ecefPos(i) = ecefPosVel(i);
00376 ecefVel(i) = ecefPosVel(i+3);
00377 }
00378
00379 Vector<double> j2kPos = transpose(c2t) * ecefPos;
00380 Vector<double> j2kVel = transpose(c2t) * ecefVel
00381 +transpose(dc2t)* ecefPos;
00382
00383 Vector<double> j2kPosVel(6,0.0);
00384 for(int i=0; i<3; i++)
00385 {
00386 j2kPosVel(i) = j2kPos(i);
00387 j2kPosVel(i+3) = j2kVel(i);
00388 }
00389
00390 return j2kPosVel;
00391
00392 }
00393
00395 Vector<double> ReferenceFrames::J2kStateToECEF(UTCTime UTC, Vector<double> j2kState)
00396 throw(Exception)
00397 {
00398
00399 Matrix<double> POM, Theta, NP;
00400 J2kToECEFMatrix(UTC,POM,Theta,NP);
00401
00402
00403 double dera1 = earthRotationAngleRate1(UTC.asTT().MJD());
00404 double dera2 = earthRotationAngleRate2(UTC.asTT().MJD());
00405 double dera3 = earthRotationAngleRate3(UTC.asTT().MJD());
00406
00407 double cs1[3][3]={{0,1,0},{-1,0,0},{0,0,0}};
00408 double cs2[3][3]={{-1,0,0},{0,-1,0},{0,0,0}};
00409 double cs3[3][3]={{0,-1,0},{1,0,0},{0,0,0}};
00410
00411 Matrix<double> s1(3,3,0.0),s2(3,3,0.0),s3(3,3,0.0);
00412 s1 = &cs1[0][0];
00413 s2 = &cs2[0][0];
00414 s3 = &cs3[0][0];
00415
00416
00417 Matrix<double> dTheta1 = s1 * Theta * dera1;
00418
00419 Matrix<double> dTheta2 = s2 * Theta * ( dera1 * dera1)
00420 + dTheta1*dera2;
00421
00422 Matrix<double> dTheta3 = s3 * Theta * ( dera1 * dera1 * dera1)
00423 + s2 * Theta * (2.0 * dera1 * dera2)
00424 + dTheta2 * dera2
00425 + dTheta1 * dera3;
00426
00427 Vector<double> r(3,0.0),v(3,0.0),a(3,0.0),d(3,0.0);
00428 for(int i=0; i<3; i++)
00429 {
00430 r(i) = j2kState(i+0);
00431 v(i) = j2kState(i+3);
00432 a(i) = j2kState(i+6);
00433 d(i) = j2kState(i+9);
00434 }
00435
00436
00437 Matrix<double> tm1 = POM * Theta * NP;
00438
00439 Matrix<double> tm2 = POM * dTheta1 * NP;
00440
00441 Matrix<double> tm3 = POM * dTheta2 * NP;
00442
00443 Matrix<double> tm4 = POM * dTheta3 * NP;
00444
00445 Vector<double> r2, v2, a2, d2;
00446
00447
00448 r2 = tm1 * r;
00449
00450
00451 v2 = tm1 * v
00452 + tm2 * r;
00453
00454
00455 a2 = tm1 * a
00456 + tm2 * v * 2.0
00457 + tm3 * r;
00458
00459
00460 d2 = tm1 * d
00461 + tm2 * a * 3.0
00462 + tm3 * v * 3.0
00463 + tm4 * r;
00464
00465 Vector<double> state(12,0.0);
00466 for(int i=0; i<3; i++)
00467 {
00468 state(i+0) = r2(i);
00469 state(i+3) = v2(i);
00470 state(i+6) = a2(i);
00471 state(i+9) = d2(i);
00472 }
00473
00474 return state;
00475
00476 }
00477
00478
00480 Vector<double> ReferenceFrames::ECEFStateToJ2k(UTCTime UTC, Vector<double> ecefState)
00481 throw(Exception)
00482 {
00483
00484 Matrix<double> POM, Theta, NP;
00485 J2kToECEFMatrix(UTC,POM,Theta,NP);
00486
00487
00488 double dera1 = earthRotationAngleRate1(UTC.asTT().MJD());
00489 double dera2 = earthRotationAngleRate2(UTC.asTT().MJD());
00490 double dera3 = earthRotationAngleRate3(UTC.asTT().MJD());
00491
00492 double cs1[3][3]={{0,1,0},{-1,0,0},{0,0,0}};
00493 double cs2[3][3]={{-1,0,0},{0,-1,0},{0,0,0}};
00494 double cs3[3][3]={{0,-1,0},{1,0,0},{0,0,0}};
00495
00496 Matrix<double> s1(3,3,0.0),s2(3,3,0.0),s3(3,3,0.0);
00497 s1 = &cs1[0][0];
00498 s2 = &cs2[0][0];
00499 s3 = &cs3[0][0];
00500
00501
00502 Matrix<double> dTheta1 = s1 * Theta * dera1;
00503
00504 Matrix<double> dTheta2 = s2 * Theta * ( dera1 * dera1)
00505 + dTheta1*dera2;
00506
00507 Matrix<double> dTheta3 = s3 * Theta * ( dera1 * dera1 * dera1)
00508 + s2 * Theta * (2.0 * dera1 * dera2)
00509 + dTheta2 * dera2
00510 + dTheta1 * dera3;
00511
00512 Vector<double> r(3,0.0),v(3,0.0),a(3,0.0),d(3,0.0);
00513 for(int i=0; i<3; i++)
00514 {
00515 r(i) = ecefState(i+0);
00516 v(i) = ecefState(i+3);
00517 a(i) = ecefState(i+6);
00518 d(i) = ecefState(i+9);
00519 }
00520
00521
00522 Matrix<double> tm1 = transpose( POM * Theta * NP );
00523
00524 Matrix<double> tm2 = transpose( POM * dTheta1 * NP );
00525
00526 Matrix<double> tm3 = transpose( POM * dTheta2 * NP );
00527
00528 Matrix<double> tm4 = transpose( POM * dTheta3 * NP );
00529
00530 Vector<double> r2, v2, a2, d2;
00531
00532
00533 r2 = tm1 * r;
00534
00535
00536 v2 = tm1 * v
00537 + tm2 * r;
00538
00539
00540 a2 = tm1 * a
00541 + tm2 * v * 2.0
00542 + tm3 * r;
00543
00544
00545 d2 = tm1 * d
00546 + tm2 * a * 3.0
00547 + tm3 * v * 3.0
00548 + tm4 * r;
00549
00550 Vector<double> state(12,0.0);
00551 for(int i=0; i<3; i++)
00552 {
00553 state(i+0) = r2(i);
00554 state(i+3) = v2(i);
00555 state(i+6) = a2(i);
00556 state(i+9) = d2(i);
00557 }
00558
00559 return state;
00560
00561 }
00562
00563
00564
00565 double ReferenceFrames::earthRotationAngle(CommonTime UT1)
00566 {
00567
00568 double t = static_cast<Epoch>(UT1).MJD() + (JD_TO_MJD - DJ00);
00569 double f = fmod(double(static_cast<Epoch>(UT1).MJD()),1.0) + fmod(JD_TO_MJD, 1.0);
00570
00571 double era = normalizeAngle(D2PI*(f+0.7790572732640+0.00273781191135448*t));
00572
00573 return era;
00574 }
00575
00576
00577
00578
00579
00580 double ReferenceFrames::earthRotationAngleRate1(const double& mjdTT)
00581 {
00582 double T = (mjdTT + (JD_TO_MJD - DJ00) )/36525.0;
00583 double dera = (1.002737909350795 + 5.9006e-11 * T - 5.9e-15 * T * T )
00584 * D2PI / 86400.0;
00585
00586 return dera;
00587 }
00588
00589
00590
00591
00592
00593
00594 double ReferenceFrames::earthRotationAngleRate2(const double& mjdTT)
00595 {
00596 double T = ( mjdTT + (JD_TO_MJD - DJ00) ) / 36525.0;
00597 double dera = (5.9006e-11 - 5.9e-15 * T) * D2PI / 86400.0;
00598
00599 return dera;
00600 }
00601
00602
00603
00604
00605
00606
00607 double ReferenceFrames::earthRotationAngleRate3(const double& mjdTT)
00608 {
00609 double T = ( mjdTT + (JD_TO_MJD - DJ00) ) / 36525.0;
00610 double dera = ( -5.9e-15 ) * D2PI / 86400.0;
00611
00612 return dera;
00613 }
00614
00639 void ReferenceFrames::doodsonArguments(CommonTime UT1,
00640 CommonTime TT,
00641 double BETA[6],
00642 double FNUT[5])
00643 {
00644
00645 double THETA = iauGmst00(UT1,TT);
00646
00647
00648
00649
00650 double t = (static_cast<Epoch>(TT).MJD() + 2400000.5 - 2451545.0) / 36525.0;
00651
00652
00653 double temp = fmod( 485868.249036 +
00654 t * ( 1717915923.2178 +
00655 t * ( 31.8792 +
00656 t * ( 0.051635 +
00657 t * ( - 0.00024470 ) ) ) ), TURNAS ) * DAS2R;
00658
00659 double F1 = normalizeAngle(temp);
00660
00661
00662 temp = fmod( 1287104.793048 +
00663 t * ( 129596581.0481 +
00664 t * ( - 0.5532 +
00665 t * ( 0.000136 +
00666 t * ( - 0.00001149 ) ) ) ), TURNAS ) * DAS2R;
00667
00668 double F2 = normalizeAngle(temp);
00669
00670
00671 temp = fmod( 335779.526232 +
00672 t * ( 1739527262.8478 +
00673 t * ( - 12.7512 +
00674 t * ( - 0.001037 +
00675 t * ( 0.00000417 ) ) ) ), TURNAS ) * DAS2R;
00676 double F3 = normalizeAngle(temp);
00677
00678
00679 temp = fmod( 1072260.703692 +
00680 t * ( 1602961601.2090 +
00681 t * ( - 6.3706 +
00682 t * ( 0.006593 +
00683 t * ( - 0.00003169 ) ) ) ), TURNAS ) * DAS2R;
00684
00685 double F4 = normalizeAngle(temp);
00686
00687
00688 temp = fmod( 450160.398036 +
00689 t * ( - 6962890.5431 +
00690 t * ( 7.4722 +
00691 t * ( 0.007702 +
00692 t * ( - 0.00005939 ) ) ) ), TURNAS ) * DAS2R;
00693 double F5 = normalizeAngle(temp);
00694
00695 FNUT[0] = F1;
00696 FNUT[1] = F2;
00697 FNUT[2] = F3;
00698 FNUT[3] = F4;
00699 FNUT[4] = F5;
00700
00701
00702 double S = F3+F5;
00703
00704 BETA[0] = THETA+ASConstant::PI-S;
00705 BETA[1] = F3+F5;
00706 BETA[2] = S-F4;
00707 BETA[3] = S-F1;
00708 BETA[4] = -F5;
00709 BETA[5] = S-F4-F2;
00710
00711 }
00712
00713
00714 void ReferenceFrames::test()
00715 {
00716 IERS::loadSTKFile("InputData/EOP-v1.1.txt");
00717
00718
00719 double rv_j2k[6]={-23830.593e3,-9747.074e3,-6779.829e3,
00720 +1.561964e3,-1.754346e3,-3.068851e3};
00721
00722 Vector<double> j2kPosVel(6,0.0);
00723 Vector<double> ecefPosVel(6,0.0);
00724
00725
00726 UTCTime utc(2007,07,01,00,0,0.0);
00727
00728 j2kPosVel = rv_j2k;
00729 ecefPosVel = J2kPosVelToECEF(utc,j2kPosVel);
00730
00731 for(int i=0;i<6;i++)
00732 cout<<setprecision(12)<<ecefPosVel(i)<<endl;
00733
00734 int a =0;
00735
00736
00737
00738
00739
00740
00741
00742
00743
00744
00745
00746
00747
00748
00749
00750
00751
00752
00753
00754
00755
00756
00757
00758
00759
00760
00761
00762
00763
00764
00765
00766
00767
00768
00769
00770
00771
00772
00773
00774
00775
00776
00777
00778 }
00779
00780
00781 double ReferenceFrames::normalizeAngle(double a)
00782 {
00783 double w = fmod(a, D2PI);
00784 if (fabs(w) >= (D2PI*0.5))
00785 {
00786 w-= ((a<0.0)?-D2PI:D2PI);
00787 }
00788
00789 return w;
00790 }
00791
00792
00793 Matrix<double> ReferenceFrames::Rx(const double& angle)
00794 {
00795 const double s = std::sin(angle);
00796 const double c = std::cos(angle);
00797
00798 const double a[9] = { 1, 0, 0, 0, c, s, 0,-s, c };
00799
00800 Matrix<double> r(3,3,0.0);
00801 r = a;
00802
00803 return r;
00804 }
00805
00806
00807 Matrix<double> ReferenceFrames::Ry(const double& angle)
00808 {
00809 const double s = std::sin(angle);
00810 const double c = std::cos(angle);
00811
00812 const double a[9] = { c, 0,-s, 0, 1, 0, s, 0, c };
00813
00814 Matrix<double> r(3,3,0.0);
00815 r = a;
00816
00817 return r;
00818 }
00819
00820
00821 Matrix<double> ReferenceFrames::Rz(const double& angle)
00822 {
00823 const double s = std::sin(angle);
00824 const double c = std::cos(angle);
00825
00826 const double a[9] = { c, s, 0,-s, c, 0, 0, 0, 1 };
00827
00828 Matrix<double> r(3,3,0.0);
00829 r = a;
00830
00831 return r;
00832 }
00833
00834 Matrix<double> ReferenceFrames::iauPmat76(CommonTime TT)
00835 {
00836
00837
00838 const double t0 = 0.0;
00839
00840
00841 const double t = (JD_TO_MJD - DJ00 + static_cast<Epoch>(TT).MJD()) / DJC;
00842
00843
00844 const double tas2r = t * DAS2R;
00845 const double w = 2306.2181 + (1.39656 - 0.000139 * t0) * t0;
00846
00847 double zeta = (w + ((0.30188 - 0.000344 * t0) + 0.017998 * t) * t) * tas2r;
00848
00849 double z = (w + ((1.09468 + 0.000066 * t0) + 0.018203 * t) * t) * tas2r;
00850
00851 double theta = ((2004.3109 + (-0.85330 - 0.000217 * t0) * t0)
00852 + ((-0.42665 - 0.000217 * t0) - 0.041833 * t) * t) * tas2r;
00853
00854 return ( Rz(-z) * Ry(theta) * Rz(-zeta) );
00855
00856 }
00857
00858
00859 void ReferenceFrames::nutationAngles(CommonTime TT, double& dpsi, double& deps)
00860 {
00861
00862 const double U2R = DAS2R / 1e4;
00863
00864
00865
00866
00867
00868
00869
00870
00871 static const struct
00872 {
00873 int nl,nlp,nf,nd,nom;
00874 double sp,spt;
00875 double ce,cet;
00876 } x[] = {
00877
00878
00879 { 0, 0, 0, 0, 1, -171996.0, -174.2, 92025.0, 8.9 },
00880 { 0, 0, 0, 0, 2, 2062.0, 0.2, -895.0, 0.5 },
00881 { -2, 0, 2, 0, 1, 46.0, 0.0, -24.0, 0.0 },
00882 { 2, 0, -2, 0, 0, 11.0, 0.0, 0.0, 0.0 },
00883 { -2, 0, 2, 0, 2, -3.0, 0.0, 1.0, 0.0 },
00884 { 1, -1, 0, -1, 0, -3.0, 0.0, 0.0, 0.0 },
00885 { 0, -2, 2, -2, 1, -2.0, 0.0, 1.0, 0.0 },
00886 { 2, 0, -2, 0, 1, 1.0, 0.0, 0.0, 0.0 },
00887 { 0, 0, 2, -2, 2, -13187.0, -1.6, 5736.0, -3.1 },
00888 { 0, 1, 0, 0, 0, 1426.0, -3.4, 54.0, -0.1 },
00889
00890
00891 { 0, 1, 2, -2, 2, -517.0, 1.2, 224.0, -0.6 },
00892 { 0, -1, 2, -2, 2, 217.0, -0.5, -95.0, 0.3 },
00893 { 0, 0, 2, -2, 1, 129.0, 0.1, -70.0, 0.0 },
00894 { 2, 0, 0, -2, 0, 48.0, 0.0, 1.0, 0.0 },
00895 { 0, 0, 2, -2, 0, -22.0, 0.0, 0.0, 0.0 },
00896 { 0, 2, 0, 0, 0, 17.0, -0.1, 0.0, 0.0 },
00897 { 0, 1, 0, 0, 1, -15.0, 0.0, 9.0, 0.0 },
00898 { 0, 2, 2, -2, 2, -16.0, 0.1, 7.0, 0.0 },
00899 { 0, -1, 0, 0, 1, -12.0, 0.0, 6.0, 0.0 },
00900 { -2, 0, 0, 2, 1, -6.0, 0.0, 3.0, 0.0 },
00901
00902
00903 { 0, -1, 2, -2, 1, -5.0, 0.0, 3.0, 0.0 },
00904 { 2, 0, 0, -2, 1, 4.0, 0.0, -2.0, 0.0 },
00905 { 0, 1, 2, -2, 1, 4.0, 0.0, -2.0, 0.0 },
00906 { 1, 0, 0, -1, 0, -4.0, 0.0, 0.0, 0.0 },
00907 { 2, 1, 0, -2, 0, 1.0, 0.0, 0.0, 0.0 },
00908 { 0, 0, -2, 2, 1, 1.0, 0.0, 0.0, 0.0 },
00909 { 0, 1, -2, 2, 0, -1.0, 0.0, 0.0, 0.0 },
00910 { 0, 1, 0, 0, 2, 1.0, 0.0, 0.0, 0.0 },
00911 { -1, 0, 0, 1, 1, 1.0, 0.0, 0.0, 0.0 },
00912 { 0, 1, 2, -2, 0, -1.0, 0.0, 0.0, 0.0 },
00913
00914
00915 { 0, 0, 2, 0, 2, -2274.0, -0.2, 977.0, -0.5 },
00916 { 1, 0, 0, 0, 0, 712.0, 0.1, -7.0, 0.0 },
00917 { 0, 0, 2, 0, 1, -386.0, -0.4, 200.0, 0.0 },
00918 { 1, 0, 2, 0, 2, -301.0, 0.0, 129.0, -0.1 },
00919 { 1, 0, 0, -2, 0, -158.0, 0.0, -1.0, 0.0 },
00920 { -1, 0, 2, 0, 2, 123.0, 0.0, -53.0, 0.0 },
00921 { 0, 0, 0, 2, 0, 63.0, 0.0, -2.0, 0.0 },
00922 { 1, 0, 0, 0, 1, 63.0, 0.1, -33.0, 0.0 },
00923 { -1, 0, 0, 0, 1, -58.0, -0.1, 32.0, 0.0 },
00924 { -1, 0, 2, 2, 2, -59.0, 0.0, 26.0, 0.0 },
00925
00926
00927 { 1, 0, 2, 0, 1, -51.0, 0.0, 27.0, 0.0 },
00928 { 0, 0, 2, 2, 2, -38.0, 0.0, 16.0, 0.0 },
00929 { 2, 0, 0, 0, 0, 29.0, 0.0, -1.0, 0.0 },
00930 { 1, 0, 2, -2, 2, 29.0, 0.0, -12.0, 0.0 },
00931 { 2, 0, 2, 0, 2, -31.0, 0.0, 13.0, 0.0 },
00932 { 0, 0, 2, 0, 0, 26.0, 0.0, -1.0, 0.0 },
00933 { -1, 0, 2, 0, 1, 21.0, 0.0, -10.0, 0.0 },
00934 { -1, 0, 0, 2, 1, 16.0, 0.0, -8.0, 0.0 },
00935 { 1, 0, 0, -2, 1, -13.0, 0.0, 7.0, 0.0 },
00936 { -1, 0, 2, 2, 1, -10.0, 0.0, 5.0, 0.0 },
00937
00938
00939 { 1, 1, 0, -2, 0, -7.0, 0.0, 0.0, 0.0 },
00940 { 0, 1, 2, 0, 2, 7.0, 0.0, -3.0, 0.0 },
00941 { 0, -1, 2, 0, 2, -7.0, 0.0, 3.0, 0.0 },
00942 { 1, 0, 2, 2, 2, -8.0, 0.0, 3.0, 0.0 },
00943 { 1, 0, 0, 2, 0, 6.0, 0.0, 0.0, 0.0 },
00944 { 2, 0, 2, -2, 2, 6.0, 0.0, -3.0, 0.0 },
00945 { 0, 0, 0, 2, 1, -6.0, 0.0, 3.0, 0.0 },
00946 { 0, 0, 2, 2, 1, -7.0, 0.0, 3.0, 0.0 },
00947 { 1, 0, 2, -2, 1, 6.0, 0.0, -3.0, 0.0 },
00948 { 0, 0, 0, -2, 1, -5.0, 0.0, 3.0, 0.0 },
00949
00950
00951 { 1, -1, 0, 0, 0, 5.0, 0.0, 0.0, 0.0 },
00952 { 2, 0, 2, 0, 1, -5.0, 0.0, 3.0, 0.0 },
00953 { 0, 1, 0, -2, 0, -4.0, 0.0, 0.0, 0.0 },
00954 { 1, 0, -2, 0, 0, 4.0, 0.0, 0.0, 0.0 },
00955 { 0, 0, 0, 1, 0, -4.0, 0.0, 0.0, 0.0 },
00956 { 1, 1, 0, 0, 0, -3.0, 0.0, 0.0, 0.0 },
00957 { 1, 0, 2, 0, 0, 3.0, 0.0, 0.0, 0.0 },
00958 { 1, -1, 2, 0, 2, -3.0, 0.0, 1.0, 0.0 },
00959 { -1, -1, 2, 2, 2, -3.0, 0.0, 1.0, 0.0 },
00960 { -2, 0, 0, 0, 1, -2.0, 0.0, 1.0, 0.0 },
00961
00962
00963 { 3, 0, 2, 0, 2, -3.0, 0.0, 1.0, 0.0 },
00964 { 0, -1, 2, 2, 2, -3.0, 0.0, 1.0, 0.0 },
00965 { 1, 1, 2, 0, 2, 2.0, 0.0, -1.0, 0.0 },
00966 { -1, 0, 2, -2, 1, -2.0, 0.0, 1.0, 0.0 },
00967 { 2, 0, 0, 0, 1, 2.0, 0.0, -1.0, 0.0 },
00968 { 1, 0, 0, 0, 2, -2.0, 0.0, 1.0, 0.0 },
00969 { 3, 0, 0, 0, 0, 2.0, 0.0, 0.0, 0.0 },
00970 { 0, 0, 2, 1, 2, 2.0, 0.0, -1.0, 0.0 },
00971 { -1, 0, 0, 0, 2, 1.0, 0.0, -1.0, 0.0 },
00972 { 1, 0, 0, -4, 0, -1.0, 0.0, 0.0, 0.0 },
00973
00974
00975 { -2, 0, 2, 2, 2, 1.0, 0.0, -1.0, 0.0 },
00976 { -1, 0, 2, 4, 2, -2.0, 0.0, 1.0, 0.0 },
00977 { 2, 0, 0, -4, 0, -1.0, 0.0, 0.0, 0.0 },
00978 { 1, 1, 2, -2, 2, 1.0, 0.0, -1.0, 0.0 },
00979 { 1, 0, 2, 2, 1, -1.0, 0.0, 1.0, 0.0 },
00980 { -2, 0, 2, 4, 2, -1.0, 0.0, 1.0, 0.0 },
00981 { -1, 0, 4, 0, 2, 1.0, 0.0, 0.0, 0.0 },
00982 { 1, -1, 0, -2, 0, 1.0, 0.0, 0.0, 0.0 },
00983 { 2, 0, 2, -2, 1, 1.0, 0.0, -1.0, 0.0 },
00984 { 2, 0, 2, 2, 2, -1.0, 0.0, 0.0, 0.0 },
00985
00986
00987 { 1, 0, 0, 2, 1, -1.0, 0.0, 0.0, 0.0 },
00988 { 0, 0, 4, -2, 2, 1.0, 0.0, 0.0, 0.0 },
00989 { 3, 0, 2, -2, 2, 1.0, 0.0, 0.0, 0.0 },
00990 { 1, 0, 2, -2, 0, -1.0, 0.0, 0.0, 0.0 },
00991 { 0, 1, 2, 0, 1, 1.0, 0.0, 0.0, 0.0 },
00992 { -1, -1, 0, 2, 1, 1.0, 0.0, 0.0, 0.0 },
00993 { 0, 0, -2, 0, 1, -1.0, 0.0, 0.0, 0.0 },
00994 { 0, 0, 2, -1, 2, -1.0, 0.0, 0.0, 0.0 },
00995 { 0, 1, 0, 2, 0, -1.0, 0.0, 0.0, 0.0 },
00996 { 1, 0, -2, -2, 0, -1.0, 0.0, 0.0, 0.0 },
00997
00998
00999 { 0, -1, 2, 0, 1, -1.0, 0.0, 0.0, 0.0 },
01000 { 1, 1, 0, -2, 1, -1.0, 0.0, 0.0, 0.0 },
01001 { 1, 0, -2, 2, 0, -1.0, 0.0, 0.0, 0.0 },
01002 { 2, 0, 0, 2, 0, 1.0, 0.0, 0.0, 0.0 },
01003 { 0, 0, 2, 4, 2, -1.0, 0.0, 0.0, 0.0 },
01004 { 0, 1, 0, 1, 0, 1.0, 0.0, 0.0, 0.0 }
01005 };
01006
01007
01008 const int NT = (int) (sizeof x / sizeof x[0]);
01009
01010
01011
01012 const double t = ((JD_TO_MJD - DJ00) + static_cast<Epoch>(TT).MJD()) / DJC;
01013
01014
01015
01016
01017
01018 double el = normalizeAngle(
01019 (485866.733 + (715922.633 + (31.310 + 0.064 * t) * t) * t)
01020 * DAS2R + fmod(1325.0 * t, 1.0) * D2PI);
01021
01022
01023 double elp = normalizeAngle(
01024 (1287099.804 + (1292581.224 + (-0.577 - 0.012 * t) * t) * t)
01025 * DAS2R + fmod(99.0 * t, 1.0) * D2PI);
01026
01027
01028 double f = normalizeAngle(
01029 (335778.877 + (295263.137 + (-13.257 + 0.011 * t) * t) * t)
01030 * DAS2R + fmod(1342.0 * t, 1.0) * D2PI);
01031
01032
01033 double d = normalizeAngle(
01034 (1072261.307 + (1105601.328 + (-6.891 + 0.019 * t) * t) * t)
01035 * DAS2R + fmod(1236.0 * t, 1.0) * D2PI);
01036
01037
01038
01039 double om = normalizeAngle(
01040 (450160.280 + (-482890.539 + (7.455 + 0.008 * t) * t) * t)
01041 * DAS2R + fmod(-5.0 * t, 1.0) * D2PI);
01042
01043
01044
01045
01046
01047
01048 double dp = 0.0;
01049 double de = 0.0;
01050
01051
01052 for (int j = NT-1; j >= 0; j--)
01053 {
01054
01055
01056 double arg = (double)x[j].nl * el
01057 + (double)x[j].nlp * elp
01058 + (double)x[j].nf * f
01059 + (double)x[j].nd * d
01060 + (double)x[j].nom * om;
01061
01062
01063 double s = x[j].sp + x[j].spt * t;
01064 double c = x[j].ce + x[j].cet * t;
01065 if (s != 0.0) dp += s * std::sin(arg);
01066 if (c != 0.0) de += c * std::cos(arg);
01067 }
01068
01069
01070 dpsi = dp * U2R;
01071 deps = de * U2R;
01072
01073 }
01074
01075
01076 double ReferenceFrames::meanObliquity(CommonTime TT)
01077 {
01078
01079
01080 double t = ((JD_TO_MJD - DJ00) + static_cast<Epoch>(TT).MJD()) / DJC;
01081
01082
01083 double eps0 = DAS2R * (84381.448 +
01084 (-46.8150 +
01085 (-0.00059 +
01086 ( 0.001813) * t) * t) * t);
01087
01088 return eps0;
01089 }
01090
01091 double ReferenceFrames::iauEqeq94(CommonTime TT)
01092 {
01093
01094 double t = ((JD_TO_MJD - DJ00) + static_cast<Epoch>(TT).MJD()) / DJC;
01095
01096
01097
01098 double om = normalizeAngle((450160.280 + (-482890.539
01099 + (7.455 + 0.008 * t) * t) * t) * DAS2R
01100 + fmod(-5.0 * t, 1.0) * D2PI);
01101
01102
01103 double dpsi(0.0), deps(0.0);
01104 nutationAngles(TT, dpsi, deps);
01105
01106 double eps0 = meanObliquity(TT);
01107
01108
01109 double ee = dpsi * std::cos(eps0)
01110 + DAS2R*(0.00264 * std::sin(om) + 0.000063 * std::sin(om + om));
01111
01112 return ee;
01113 }
01114
01115 double ReferenceFrames::iauGmst82(CommonTime UT1)
01116 {
01117
01118 const double A = 24110.54841 - 86400.0 / 2.0;
01119 const double B = 8640184.812866;
01120 const double C = 0.093104;
01121 const double D = -6.2e-6;
01122
01123
01124
01125
01126
01127
01128 double d2 = JD_TO_MJD;
01129 double d1 = static_cast<Epoch>(UT1).MJD();
01130 double t = (d1 + (d2 - DJ00)) / DJC;
01131
01132
01133 double f = 86400.0 * (fmod(d1, 1.0) + fmod(d2, 1.0));
01134
01135
01136 double gmst = normalizeAngle(
01137 DS2R * ((A + (B + (C + D * t) * t) * t) + f));
01138
01139 return gmst;
01140
01141 }
01142
01143
01144 double ReferenceFrames::iauGmst00(CommonTime UT1,CommonTime TT)
01145 {
01146
01147
01148 double t = ((JD_TO_MJD - DJ00) + static_cast<Epoch>(TT).MJD()) / DJC;
01149
01150 double gmst = normalizeAngle(earthRotationAngle(UT1) +
01151 ( 0.014506 +
01152 ( 4612.15739966 +
01153 ( 1.39667721 +
01154 ( -0.00009344 +
01155 ( 0.00001882 )
01156 * t) * t) * t) * t) * DAS2R);
01157
01158 return gmst;
01159
01160 }
01161
01162
01163 Matrix<double> ReferenceFrames::iauNmat(const double& epsa,
01164 const double& dpsi,
01165 const double& deps)
01166 {
01167 return ( Rx(-(epsa+deps)) * Rz(-dpsi) * Rx(epsa) );
01168 }
01169
01170
01171 Matrix<double> ReferenceFrames::enuMatrix(double longitude, double latitude)
01172 {
01173 const double sb = std::sin(latitude);
01174 const double cb = std::cos(latitude);
01175 const double sl = std::sin(longitude);
01176 const double cl = std::cos(longitude);
01177
01178 double r[3][3]={{-sl,cl,0.0},{-sb*cl,-sb*sl, cb},{ cb*cl,cb*sl,sb}};
01179
01180 Matrix<double> enuMat(3,3,0.0);
01181 enuMat = &r[0][0];
01182
01183 return enuMat;
01184
01185 }
01186
01187
01188 Vector<double> ReferenceFrames::enuToAzElDt(Vector<double> enu)
01189 {
01190 gpstk::Vector<double> r(3,0.0);
01191
01192 const double rho = std::sqrt(enu(0)*enu(0)+enu(1)*enu(1));
01193
01194
01195 double A = std::atan2(enu(0),enu(1));
01196 A = (A<0.0)? (A+ASConstant::TWO_PI) : A;
01197 double E = std::atan ( enu(2) / rho );
01198
01199 r(0) = A;
01200 r(1) = E;
01201 r(2) = norm(enu);
01202
01203 return r;
01204 }
01205
01206
01207 void ReferenceFrames::XYZ2BLH(double xyz[3],double blh[3])
01208 {
01209 const double f = ASConstant::f_Earth;
01210 const double R_equ = ASConstant::R_Earth;
01211 const double e2 = f*(2.0-f);
01212 const double e = std::sqrt(e2);
01213
01214 const double eps = 1.0e3*std::numeric_limits<double>::epsilon();;
01215 const double epsRequ = eps*R_equ;
01216
01217
01218 const double X = xyz[0];
01219 const double Y = xyz[1];
01220 const double Z = xyz[2];
01221 const double rho2 = X*X + Y*Y;
01222 const double rho = std::sqrt(rho2+Z*Z);
01223
01224
01225 if (rho==0.0)
01226 {
01227 blh[0] = 0.0;
01228 blh[1] = 0.0;
01229 blh[2] = -R_equ;
01230 return;
01231 }
01232
01233
01234 double dZ, dZ_new, SinPhi;
01235 double ZdZ, Nh, N;
01236
01237 dZ = e2*Z;
01238 while(1)
01239 {
01240 ZdZ = Z + dZ;
01241 Nh = std::sqrt ( rho2 + ZdZ*ZdZ );
01242 SinPhi = ZdZ / Nh;
01243 N = R_equ / std::sqrt(1.0-e2*SinPhi*SinPhi);
01244 dZ_new = N*e2*SinPhi;
01245 if ( std::fabs(dZ-dZ_new) < epsRequ ) break;
01246 dZ = dZ_new;
01247 }
01248
01249
01250 blh[0] = std::atan2 ( ZdZ, std::sqrt(rho2) );
01251 blh[1] = std::atan2 ( Y, X );
01252 blh[2] = Nh - N;
01253
01254 }
01255
01256 void ReferenceFrames::BLH2XYZ(double blh[3],double xyz[3])
01257 {
01258 const double f = ASConstant::f_Earth;
01259 const double a = ASConstant::R_Earth;
01260 const double e2 = f*(2.0-f);
01261
01262 double N=a/(std::sqrt(1-e2*std::sin(blh[0])*std::sin(blh[0])));
01263 xyz[0]=(N+blh[2])*std::cos(blh[0])*std::cos(blh[1]);
01264 xyz[1]=(N+blh[2])*std::cos(blh[0])*std::sin(blh[1]);
01265 xyz[2]=(N*(1-e2)+blh[2])*std::sin(blh[0]);
01266
01267 }
01268
01269
01270 void ReferenceFrames::XYZ2ENU(double blh[3],double xyz[3],double enu[3])
01271 {
01272 double xyz0[3]={0.0};
01273 BLH2XYZ(blh,xyz0);
01274
01275 double dxyz[3]={0.0};
01276 dxyz[0] = xyz[0]-xyz0[0];
01277 dxyz[1] = xyz[1]-xyz0[1];
01278 dxyz[2] = xyz[2]-xyz0[2];
01279
01280 const double sb = std::sin(blh[0]);
01281 const double cb = std::cos(blh[0]);
01282 const double sl = std::sin(blh[1]);
01283 const double cl = std::cos(blh[1]);
01284
01285 double r[3][3]={{-sl,cl,0.0},{-sb*cl,-sb*sl, cb},{ cb*cl,cb*sl,sb}};
01286
01287 enu[0] = r[0][0] * dxyz[0] + r[0][1] * dxyz[1] + r[0][2] * dxyz[2];
01288 enu[1] = r[1][0] * dxyz[0] + r[1][1] * dxyz[1] + r[1][2] * dxyz[2];
01289 enu[2] = r[2][0] * dxyz[0] + r[2][1] * dxyz[1] + r[2][2] * dxyz[2];
01290
01291 }
01292
01293
01294 }
01295
01296