Skip to content

Commit 3baab20

Browse files
xanthospapxanthospap
committed
minor
1 parent 80e44f0 commit 3baab20

File tree

1 file changed

+48
-36
lines changed

1 file changed

+48
-36
lines changed

‎bin/test_deriv_costg.cpp‎

Lines changed: 48 additions & 36 deletions
Original file line numberDiff line numberDiff line change
@@ -73,6 +73,8 @@ int deriv(double tsec, Eigen::Ref<const Eigen::VectorXd> y0,
7373
/* current time in TAI */
7474
dso::MjdEpoch t = params->t0();
7575
t.add_seconds(dso::FractionalSeconds(tsec));
76+
/* current time in GPST (debugging) */
77+
const auto tgps = t.tai2gps();
7678

7779
/* Dealunay args (14) */
7880
double fargs[14];
@@ -90,13 +92,18 @@ int deriv(double tsec, Eigen::Ref<const Eigen::VectorXd> y0,
9092
/* compute acceleration for given epoch/position (ECEF) */
9193
[[maybe_unused]] Eigen::Matrix<double, 3, 3> g;
9294
const Eigen::VectorXd r_ecef = R * y0.segment<3>(0);
93-
Eigen::Matrix<double, 3, 1> acc;
94-
if (dso::sh2gradient_cunningham(stokes, r_ecef, acc, g,
95+
Eigen::Matrix<double, 3, 1> acc_grav;
96+
if (dso::sh2gradient_cunningham(stokes, r_ecef, acc_grav, g,
9597
stokes.max_degree(), stokes.max_order(), -1,
9698
-1, &(params->tw()), &(params->tm()))) {
9799
fprintf(stderr, "ERROR Failed computing acceleration/gradient\n");
98100
return 1;
99101
}
102+
{
103+
const auto acc = R.transpose() * acc_grav;
104+
printf("%.15f %.15f %.15f %.15f", tgps.as_mjd(), acc(0), acc(1), acc(2));
105+
y.segment<3>(0) += acc;
106+
}
100107

101108
Eigen::Matrix<double, 3, 1> acc_moon;
102109
Eigen::Matrix<double, 3, 1> acc_sun;
@@ -108,6 +115,8 @@ int deriv(double tsec, Eigen::Ref<const Eigen::VectorXd> y0,
108115
}
109116
acc_sun = dso::point_mass_acceleration(y0.segment<3>(0),
110117
rtb_sun.segment<3>(0), GM_Sun);
118+
printf(" %.15f %.15f %.15f", acc_sun(0), acc_sun(1), acc_sun(2));
119+
y.segment<3>(0) += acc_sun;
111120

112121
/* get Moon position in ICRF */
113122
Eigen::Matrix<double, 3, 1> rtb_moon;
@@ -116,9 +125,13 @@ int deriv(double tsec, Eigen::Ref<const Eigen::VectorXd> y0,
116125
return 2;
117126
}
118127
acc_moon = dso::point_mass_acceleration(y0.segment<3>(0), rtb_moon, GM_Moon);
128+
printf(" %.15f %.15f %.15f", acc_moon(0), acc_moon(1), acc_moon(2));
129+
y.segment<3>(0) += acc_moon;
119130

120131
/* Relativistic Correction */
121132
Eigen::Matrix<double, 3, 1> acc_rel = dso::iers2010_relativistic_acceleration(y0, rtb_sun);
133+
printf(" %.15f %.15f %.15f", acc_rel(0), acc_rel(1), acc_rel(2));
134+
y.segment<3>(0) += acc_rel;
122135

123136
/* Solid Earth Tide (ITRF) */
124137
Eigen::Matrix<double, 3, 1> acc_set;
@@ -131,6 +144,11 @@ int deriv(double tsec, Eigen::Ref<const Eigen::VectorXd> y0,
131144
fprintf(stderr, "ERROR Failed computing acceleration/gradient\n");
132145
return 1;
133146
}
147+
{
148+
const auto acc = R.transpose() * acc_set;
149+
printf(" %.15f %.15f %.15f", acc(0), acc(1), acc(2));
150+
y.segment<3>(0) += acc;
151+
}
134152

135153
/* Solid Earth Pole Tide */
136154
dso::StokesCoeffs sept_stokes(3, 3);
@@ -146,6 +164,11 @@ int deriv(double tsec, Eigen::Ref<const Eigen::VectorXd> y0,
146164
fprintf(stderr, "ERROR Failed computing acceleration/gradient\n");
147165
return 1;
148166
}
167+
{
168+
const auto acc = R.transpose() * acc_setp;
169+
printf(" %.15f %.15f %.15f", acc(0), acc(1), acc(2));
170+
y.segment<3>(0) += acc;
171+
}
149172

150173
/* Ocean Pole Tide */
151174
Eigen::Matrix<double, 3, 1> acc_otp;
@@ -164,65 +187,53 @@ int deriv(double tsec, Eigen::Ref<const Eigen::VectorXd> y0,
164187
fprintf(stderr, "ERROR Failed computing acceleration/gradient\n");
165188
return 1;
166189
}
190+
{
191+
const auto acc = R.transpose() * acc_otp;
192+
printf(" %.15f %.15f %.15f", acc(0), acc(1), acc(2));
193+
y.segment<3>(0) += acc;
194+
}
167195

168196
/* Dealiasing */
169197
Eigen::Matrix<double, 3, 1> acc_das;
170198
if (params->mdealias->coefficients_at(dso::from_mjdepoch<dso::nanoseconds>(t.tai2tt()), stokes)) {
171199
fprintf(stderr, "Failed interpolating dealiasing coefficients\n");
172200
return 1;
173201
}
174-
/* TODO
175-
* This is only needed for testing vs COSTG and should be removed from
176-
* "real" code
177-
*/
178-
stokes.C(0,0) = 0e0;
179-
stokes.C(1,0) = stokes.C(1,1) = 0e0;
180-
stokes.S(1,1) = 0e0;
202+
stokes.C(0, 0) = 0e0;
203+
stokes.C(1, 0) = stokes.C(1, 1) = 0e0;
204+
stokes.S(1, 1) = 0e0;
181205
if (dso::sh2gradient_cunningham(stokes, r_ecef,
182206
acc_das, g, params->mdealias_maxdegree,
183207
params->mdealias_maxorder, -1, -1,
184208
&(params->tw()), &(params->tm()))) {
185209
fprintf(stderr, "ERROR Failed computing acceleration/gradient\n");
186210
return 1;
187211
}
188-
189212
{
190-
// print for debugging
191-
const auto tgps = t.tai2gps();
192-
193-
// OK
194-
// printf("%.15f %.15f %.15f %.15f\n", tgps.as_mjd(), acc_moon(0), acc_moon(1), acc_moon(2));
195-
// printf("%.15f %.15f %.15f %.15f\n", tgps.as_mjd(), acc_sun(0), acc_sun(1), acc_sun(2));
196-
// const auto acc_setI = R.transpose() * acc_setp;
197-
// printf("%.15f %.15f %.15f %.15f\n", tgps.as_mjd(), acc_setI(0), acc_setI(1), acc_setI(2));
198-
// const auto acc_setI = R.transpose() * acc_otp;
199-
// printf("%.15f %.15f %.15f %.15f\n", tgps.as_mjd(), acc_setI(0), acc_setI(1), acc_setI(2));
200-
// printf("%.15f %.15f %.15f %.15f\n", tgps.as_mjd(), acc_rel(0), acc_rel(1), acc_rel(2));
201-
const auto acc_setI = R.transpose() * acc_das;
202-
printf("%.15f %.15f %.15f %.15f\n", tgps.as_mjd(), acc_setI(0), acc_setI(1), acc_setI(2));
203-
204-
// TODO
205-
// const auto acc_setI = R.transpose() * acc_set;
206-
// printf("%.15f %.15f %.15f %.15f\n", tgps.as_mjd(), acc_setI(0), acc_setI(1), acc_setI(2));
213+
const auto acc = R.transpose() * acc_das;
214+
printf(" %.15f %.15f %.15f", acc(0), acc(1), acc(2));
215+
y.segment<3>(0) += acc;
207216
}
208217

209218
/* set velocity vector (ICRF) */
219+
y.segment<3>(3) = y.segment<3>(0);
210220
y.segment<3>(0) = y0.segment<3>(3);
211221

212222
/* ECEF to ICRF note that y = (v, a) and y0 = (r, v) */
213223
// y.segment<3>(3) =
214224
// R.transpose() * acc + acc_tb;
215-
y.segment<3>(3) = (acc_moon + acc_sun + acc_rel);
216-
y.segment<3>(3) += R.transpose() * acc_set;
217-
y.segment<3>(3) += R.transpose() * acc;
225+
//y.segment<3>(3) = (acc_moon + acc_sun + acc_rel);
226+
//y.segment<3>(3) += R.transpose() * acc_set;
227+
//y.segment<3>(3) += R.transpose() * acc;
218228

229+
printf("\n");
219230
return 0;
220231
}
221232

222233
int main(int argc, char* argv[])
223234
{
224235
if (argc < 3) {
225-
fprintf(stderr, "Usage: %s CONFIG SP3_file [SAT_ID]\n", argv[0]);
236+
fprintf(stderr, "Usage: %s [CONFIG] [00orbit_itrf.sp3] \n", argv[0]);
226237
return 1;
227238
}
228239

@@ -330,8 +341,8 @@ int main(int argc, char* argv[])
330341
}
331342

332343
/* setup the integrator */
333-
dso::Dop853 dop853(deriv, 6, &params, 1e-9, 1e-12);
334-
dop853.set_stiffness_check(10);
344+
// dso::Dop853 dop853(deriv, 6, &params, 1e-9, 1e-12);
345+
// dop853.set_stiffness_check(10);
335346

336347
/* dummy */
337348
double fargs[14];
@@ -345,6 +356,7 @@ int main(int argc, char* argv[])
345356
dso::Sp3DataBlock block;
346357
int sp3err = 0;
347358
while (!sp3err) {
359+
/* get next redord from sp3 */
348360
sp3err = sp3.get_next_data_block(sv, block);
349361
if (sp3err > 0) {
350362
printf("Something went wrong ....status = %3d\n", sp3err);
@@ -368,15 +380,15 @@ int main(int argc, char* argv[])
368380
state = y;
369381
/* seconds since initial epoch */
370382
dso::FractionalSeconds sec = block_tai.diff<dso::DateTimeDifferenceType::FractionalSeconds>(start_t);
371-
/* compute derivative at this epoch */
383+
/* compute accelerations at this epoch */
372384
if (deriv(sec.seconds(), state, &params, y)) {
373385
fprintf(stderr, "ERROR. Failed computing derivative!\n");
374386
return 1;
375387
}
376388
++it;
377-
if (it >= 2000)
378-
break;
379389
}
380390

391+
printf("# Num of epochs parsed/used: %ld\n", it);
392+
381393
return sp3err;
382394
}

0 commit comments

Comments
 (0)