Skip to content

Commit a95a5c4

Browse files
xanthospapxanthospap
committed
earth rotation
1 parent d009f0d commit a95a5c4

File tree

1 file changed

+73
-34
lines changed

1 file changed

+73
-34
lines changed

‎bin/test_deriv_costg.cpp‎

Lines changed: 73 additions & 34 deletions
Original file line numberDiff line numberDiff line change
@@ -16,17 +16,20 @@
1616
constexpr const double GM_Moon = /*0.49028010560e13;*/ 4902.800076e9;
1717
constexpr const double GM_Sun = /*1.32712442076e20;*/ 132712440040.944e9;
1818

19-
/* Compute and return thr rotation matrix and its derivative between GCRF and
19+
/* Compute and return the rotation matrix and its derivative between GCRF and
2020
* ECEF frames.
2121
*/
22-
int gcrf2ecef(const dso::MjdEpoch &tai, dso::EopSeries &eops,
23-
Eigen::Matrix<double, 3, 3> &R, Eigen::Matrix<double, 3, 3> &dRdt,
24-
double *fargs, dso::EopRecord &eopr) noexcept {
22+
int prep_c2i(const dso::MjdEpoch &tai, dso::EopSeries &eops,
23+
Eigen::Quaterniond &q_c2tirs, Eigen::Quaterniond &q_tirs2i,
24+
double *fargs, dso::EopRecord &eopr) noexcept {
25+
2526
const auto tt = tai.tai2tt();
26-
double X, Y;
2727

28-
/* compute (X, Y)_{cip} and (14) fundamental arguments */
29-
dso::xycip06a(tt, X, Y, fargs);
28+
/* compute (X,Y) CIP and fundamental arguments (we are doing this here
29+
* to compute fargs).
30+
*/
31+
double Xcip, Ycip;
32+
dso::xycip06a(tt, Xcip, Ycip, fargs);
3033

3134
/* get (interpolate EOPs) */
3235
if (dso::EopSeries::out_of_bounds(eops.interpolate(tt, eopr))) {
@@ -59,9 +62,31 @@ int gcrf2ecef(const dso::MjdEpoch &tai, dso::EopSeries &eops,
5962
eopr.lod() += dlod * 1e-6;
6063
}
6164

62-
R = dso::detail::gcrs2itrs(
63-
dso::era00(tt.tt2ut1(eopr.dut())), dso::s06(tt, X, Y), dso::sp00(tt), X,
64-
Y, dso::sec2rad(eopr.xp()), dso::sec2rad(eopr.yp()), eopr.lod(), dRdt);
65+
/* de-regularize */
66+
{
67+
double ut1_cor;
68+
double lod_cor;
69+
double omega_cor;
70+
dso::deop_zonal_tide(fargs, ut1_cor, lod_cor, omega_cor);
71+
/* apply (note: microseconds to seconds) */
72+
eopr.dut() += (ut1_cor * 1e-6);
73+
eopr.lod() += (lod_cor * 1e-6);
74+
}
75+
76+
/* use fundamental arguments to compute s */
77+
const double s = dso::s06(tt, Xcip, Ycip, fargs);
78+
/* apply CIP corrections */
79+
Xcip += dso::sec2rad(eopr.dX());
80+
Ycip += dso::sec2rad(eopr.dY());
81+
/* spherical crd for CIP */
82+
double d, e;
83+
dso::detail::xycip2spherical(Xcip, Ycip, d, e);
84+
const double era = dso::era00(tt.tt2ut1(eopr.dut()));
85+
86+
q_c2tirs = dso::detail::c2tirs(era, s, d, e);
87+
q_tirs2i = dso::detail::tirs2i(dso::sec2rad(eopr.xp()),
88+
dso::sec2rad(eopr.yp()), dso::sp00(tt));
89+
6590
return 0;
6691
}
6792

@@ -80,13 +105,11 @@ int deriv(double tsec, Eigen::Ref<const Eigen::VectorXd> y0,
80105

81106
/* Celestial to Terrestrial Matrix */
82107
dso::EopRecord eopr;
83-
Eigen::Matrix<double, 3, 3> R, dRdt;
84-
if (gcrf2ecef(dso::MjdEpoch(t), params->eops(), R, dRdt, fargs, eopr)) {
85-
return 8;
86-
}
108+
Eigen::Quaterniond q_c2tirs, q_tirs2i;
109+
prep_c2i(dso::MjdEpoch(t), params->eops(), q_c2tirs, q_tirs2i, fargs, eopr);
87110

88111
/* satellite position in ECEF */
89-
const Eigen::VectorXd r_ecef = R * y0.segment<3>(0);
112+
const Eigen::VectorXd r_ecef = q_tirs2i * (q_c2tirs * y0.segment<3>(0));
90113

91114
/* Gravity field stokes coefficients */
92115
dso::StokesCoeffs stokes(params->earth_gravity());
@@ -105,7 +128,7 @@ int deriv(double tsec, Eigen::Ref<const Eigen::VectorXd> y0,
105128
}
106129
{
107130
/* print acceleration in ECI (P1)*/
108-
const auto acc = R.transpose() * acc_grav;
131+
const auto acc = q_c2tirs.conjugate() * (q_tirs2i.conjugate() * acc_grav);
109132
printf("%.15f %.15f %.15f %.15f", tgps.as_mjd(), acc(0), acc(1), acc(2));
110133
y.segment<3>(0) += acc;
111134
}
@@ -144,9 +167,9 @@ int deriv(double tsec, Eigen::Ref<const Eigen::VectorXd> y0,
144167

145168
/* Solid Earth Tide (ITRF) */
146169
Eigen::Matrix<double, 3, 1> acc_set;
147-
params->solid_earth_tide()->stokes_coeffs(t.tai2tt(), t.tai2ut1(eopr.dut()),
148-
R * rtb_moon,
149-
R * rtb_sun.segment<3>(0), fargs);
170+
params->solid_earth_tide()->stokes_coeffs(
171+
t.tai2tt(), t.tai2ut1(eopr.dut()), q_tirs2i * (q_c2tirs * rtb_moon),
172+
q_tirs2i * (q_c2tirs * rtb_sun.segment<3>(0)), fargs);
150173
if (dso::sh2gradient_cunningham(params->solid_earth_tide()->stokes_coeffs(),
151174
r_ecef, acc_set, g, -1, -1, -1, -1,
152175
&(params->tw()), &(params->tm()))) {
@@ -155,7 +178,8 @@ int deriv(double tsec, Eigen::Ref<const Eigen::VectorXd> y0,
155178
}
156179
{
157180
/* print acceleration in ECI (P5)*/
158-
const auto acc = R.transpose() * acc_set;
181+
// const auto acc = R.transpose() * acc_set;
182+
const auto acc = q_c2tirs.conjugate() * (q_tirs2i.conjugate() * acc_set);
159183
printf(" %.15f %.15f %.15f", acc(0), acc(1), acc(2));
160184
y.segment<3>(0) += acc;
161185
}
@@ -175,7 +199,8 @@ int deriv(double tsec, Eigen::Ref<const Eigen::VectorXd> y0,
175199
}
176200
{
177201
/* print acceleration in ECI (P6)*/
178-
const auto acc = R.transpose() * acc_setp;
202+
// const auto acc = R.transpose() * acc_setp;
203+
const auto acc = q_c2tirs.conjugate() * (q_tirs2i.conjugate() * acc_setp);
179204
printf(" %.15f %.15f %.15f", acc(0), acc(1), acc(2));
180205
y.segment<3>(0) += acc;
181206
}
@@ -202,7 +227,8 @@ int deriv(double tsec, Eigen::Ref<const Eigen::VectorXd> y0,
202227
return 1;
203228
}
204229
{
205-
const auto acc = R.transpose() * acc_otp;
230+
// const auto acc = R.transpose() * acc_otp;
231+
const auto acc = q_c2tirs.conjugate() * (q_tirs2i.conjugate() * acc_otp);
206232
/* print acceleration in ECI (P7)*/
207233
printf(" %.15f %.15f %.15f", acc(0), acc(1), acc(2));
208234
y.segment<3>(0) += acc;
@@ -228,7 +254,8 @@ int deriv(double tsec, Eigen::Ref<const Eigen::VectorXd> y0,
228254
}
229255
{
230256
/* print acceleration in ECI (P8)*/
231-
const auto acc = R.transpose() * acc_das;
257+
// const auto acc = R.transpose() * acc_das;
258+
const auto acc = q_c2tirs.conjugate() * (q_tirs2i.conjugate() * acc_das);
232259
printf(" %.15f %.15f %.15f", acc(0), acc(1), acc(2));
233260
y.segment<3>(0) += acc;
234261
}
@@ -254,7 +281,8 @@ int deriv(double tsec, Eigen::Ref<const Eigen::VectorXd> y0,
254281
}
255282
{
256283
/* print acceleration in ECI (P9)*/
257-
const auto acc = R.transpose() * acc_atm;
284+
// const auto acc = R.transpose() * acc_atm;
285+
const auto acc = q_c2tirs.conjugate() * (q_tirs2i.conjugate() * acc_atm);
258286
printf(" %.15f %.15f %.15f", acc(0), acc(1), acc(2));
259287
y.segment<3>(0) += acc;
260288
}
@@ -278,7 +306,8 @@ int deriv(double tsec, Eigen::Ref<const Eigen::VectorXd> y0,
278306
}
279307
{
280308
/* print acceleration in ECI (P10)*/
281-
const auto acc = R.transpose() * acc_ot;
309+
// const auto acc = R.transpose() * acc_ot;
310+
const auto acc = q_c2tirs.conjugate() * (q_tirs2i.conjugate() * acc_ot);
282311
printf(" %.15f %.15f %.15f", acc(0), acc(1), acc(2));
283312
y.segment<3>(0) += acc;
284313
}
@@ -465,7 +494,7 @@ int main(int argc, char *argv[]) {
465494
/* Just for testing Vs costg */
466495
Eigen::VectorXd state = Eigen::Matrix<double, 6, 1>::Zero();
467496
Eigen::VectorXd y = Eigen::Matrix<double, 6, 1>::Zero();
468-
Eigen::Matrix<double, 3, 3> R, dRdt;
497+
// Eigen::Matrix<double, 3, 3> R, dRdt;
469498
std::size_t it = 0;
470499
dso::Sp3DataBlock block;
471500
int sp3err = 0;
@@ -476,32 +505,42 @@ int main(int argc, char *argv[]) {
476505
printf("Something went wrong ....status = %3d\n", sp3err);
477506
return 1;
478507
}
508+
479509
/* time of current block in TAI */
480510
auto block_tai = block.t;
481511
if (!std::strcmp(sp3.time_sys(), "GPS")) {
482512
block_tai = block_tai.gps2tai();
483513
}
514+
484515
/* GCRF to ITRF rotation matrix */
485-
if (gcrf2ecef(dso::MjdEpoch(block_tai), params.eops(), R, dRdt, fargs,
486-
eopr)) {
487-
return 8;
488-
}
516+
Eigen::Quaterniond q_c2tirs, q_tirs2i;
517+
prep_c2i(dso::MjdEpoch(block_tai), params.eops(), q_c2tirs, q_tirs2i,
518+
fargs, eopr);
519+
489520
/* get state for current epoch ITRF */
490521
state << block.state[0] * 1e3, block.state[1] * 1e3, block.state[2] * 1e3,
491522
block.state[4] * 1e-1, block.state[5] * 1e-1, block.state[6] * 1e-1;
492-
/* transform state to GCRF */
493-
y.segment<3>(0) = R.transpose() * state.segment<3>(0);
494-
y.segment<3>(3) = R.transpose() * state.segment<3>(3) +
495-
dRdt.transpose() * state.segment<3>(0);
523+
524+
/* transform state to GCRF (from ITRF) */
525+
Eigen::Vector3d omega;
526+
omega << 0e0, 0e0, dso::earth_rotation_rate(eopr.lod());
527+
y.segment<3>(0) =
528+
q_c2tirs.conjugate() * (q_tirs2i.conjugate() * state.segment<3>(0));
529+
y.segment<3>(3) = q_c2tirs.conjugate() *
530+
(q_tirs2i.conjugate() * state.segment<3>(3) +
531+
omega.cross(q_tirs2i.conjugate() * state.segment<3>(0)));
496532
state = y;
533+
497534
/* seconds since initial epoch */
498535
dso::FractionalSeconds sec =
499536
block_tai.diff<dso::DateTimeDifferenceType::FractionalSeconds>(start_t);
537+
500538
/* compute accelerations at this epoch */
501539
if (deriv(sec.seconds(), state, &params, y)) {
502540
fprintf(stderr, "ERROR. Failed computing derivative!\n");
503541
return 1;
504542
}
543+
505544
++it;
506545
}
507546

0 commit comments

Comments
 (0)