Skip to content

Commit 17db7fa

Browse files
xanthospapxanthospap
committed
dealiasing prob
1 parent d3e1688 commit 17db7fa

File tree

5 files changed

+454
-99
lines changed

5 files changed

+454
-99
lines changed

‎bin/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,7 @@
22

33
set(BIN_SOURCES
44
equations_of_motion.cpp
5+
test_deriv_costg.cpp
56
)
67

78
# Process each source file and create an executable

‎bin/config.yaml‎

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -9,3 +9,20 @@ gravity:
99
model: data/EIGEN-GRGS.RL04.MEAN-FIELD.gfc
1010
degree: 80
1111
order: 80
12+
13+
ocean-pole-tide:
14+
## Leave model name empty to not include effect
15+
## Applicable Models: 'Desai02'
16+
model: Desai02
17+
coeffs: data/desaiscopolecoef.txt
18+
degree: 180
19+
order: 180
20+
21+
dealiasing:
22+
## Leave model name empty to not include effect
23+
model: "AOD1B RL06"
24+
data-file: data/AOD1B_2008-07-03_X_06.asc
25+
data-dir: data
26+
degree: 180
27+
order: 180
28+

‎bin/equations_of_motion.cpp‎

Lines changed: 47 additions & 99 deletions
Original file line numberDiff line numberDiff line change
@@ -94,7 +94,6 @@ int deriv(double tsec, Eigen::Ref<const Eigen::VectorXd> y0,
9494
return 1;
9595
}
9696

97-
// TODO adding sun worsens results !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
9897
Eigen::Matrix<double, 3, 1> acc_moon;
9998
Eigen::Matrix<double, 3, 1> acc_sun;
10099
/* get Sun position in ICRF */
@@ -124,23 +123,15 @@ int deriv(double tsec, Eigen::Ref<const Eigen::VectorXd> y0,
124123
return 1;
125124
}
126125

127-
{
128-
// print for debugging
129-
const auto tgps = t.tai2gps();
130-
printf("%.15f %.15f %.15f %.15f\n", tgps.as_mjd(), acc_moon(0), acc_moon(1), acc_moon(2));
131-
//printf("%.15f %.15f %.15f %.15f\n", tgps.as_mjd(), acc_sun(0), acc_sun(1), acc_sun(2));
132-
//printf("%.15f %.15f %.15f %.15f\n", tgps.as_mjd(), acc_set(0), acc_set(1), acc_set(2));
133-
}
134-
135126
/* set velocity vector (ICRF) */
136127
y.segment<3>(0) = y0.segment<3>(3);
137128

138129
/* ECEF to ICRF note that y = (v, a) and y0 = (r, v) */
139130
//y.segment<3>(3) =
140131
// R.transpose() * acc + acc_tb;
141-
y.segment<3>(3) = (acc_moon + acc_sun);
142-
y.segment<3>(3) += R.transpose() * acc_set;
143-
y.segment<3>(3) += R.transpose() * acc;
132+
// y.segment<3>(3) = (acc_moon + acc_sun);
133+
// y.segment<3>(3) += R.transpose() * (acc_set + acc);
134+
y.segment<3>(3) = R.transpose() * acc;
144135

145136
return 0;
146137
}
@@ -231,7 +222,6 @@ int main(int argc, char *argv[]) {
231222
double fargs[14];
232223
dso::EopRecord eopr;
233224

234-
/* Just for testing Vs costg */
235225
Eigen::VectorXd state = Eigen::Matrix<double, 6, 1>::Zero();
236226
Eigen::VectorXd y = Eigen::Matrix<double, 6, 1>::Zero();
237227
Eigen::Matrix<double, 3, 3> R, dRdt;
@@ -244,96 +234,54 @@ int main(int argc, char *argv[]) {
244234
printf("Something went wrong ....status = %3d\n", sp3err);
245235
return 1;
246236
}
247-
/* time of current block in TAI */
248-
auto block_tai = block.t;
249-
if (!std::strcmp(sp3.time_sys(), "GPS")) {
250-
block_tai = block_tai.gps2tai();
251-
}
252-
/* GCRF to ITRF rotation matrix */
253-
if (gcrf2ecef(dso::MjdEpoch(block_tai), eop, R, dRdt, fargs, eopr)) {
254-
return 8;
255-
}
256-
/* get state for current epoch ITRF */
257-
state << block.state[0] * 1e3, block.state[1] * 1e3, block.state[2] * 1e3,
258-
block.state[4] * 1e-1, block.state[5] * 1e-1, block.state[6] * 1e-1;
259-
/* transform state to GCRF */
260-
y.segment<3>(0) = R.transpose() * state.segment<3>(0);
261-
y.segment<3>(3) = R.transpose() * state.segment<3>(3) +
262-
dRdt.transpose() * state.segment<3>(0);
263-
state = y;
264-
/* seconds since initial epoch */
265-
dso::FractionalSeconds sec =
266-
block_tai.diff<dso::DateTimeDifferenceType::FractionalSeconds>(start_t);
267-
/* compute derivative at this epoch */
268-
if (deriv(sec.seconds(), state, &params, y)) {
269-
fprintf(stderr, "ERROR. Failed computing derivative!\n");
270-
return 1;
237+
bool position_ok = !block.flag.is_set(dso::Sp3Event::bad_abscent_position);
238+
if (position_ok && (!sp3err)) {
239+
auto block_tai = block.t;
240+
if (!std::strcmp(sp3.time_sys(), "GPS")) {
241+
block_tai = block_tai.gps2tai();
242+
}
243+
if (gcrf2ecef(dso::MjdEpoch(block_tai), eop, R, dRdt, fargs, eopr)) {
244+
return 8;
245+
}
246+
if (!it) {
247+
/* first state of satellite in file; transform to celestial and store
248+
* as state and start_t. This is where we start integrating from.
249+
*/
250+
state << block.state[0] * 1e3, block.state[1] * 1e3,
251+
block.state[2] * 1e3, block.state[4] * 1e-1, block.state[5] * 1e-1,
252+
block.state[6] * 1e-1;
253+
y.segment<3>(0) = R.transpose() * state.segment<3>(0);
254+
y.segment<3>(3) = R.transpose() * state.segment<3>(3) +
255+
dRdt.transpose() * state.segment<3>(0);
256+
start_t = block_tai;
257+
state = y;
258+
} else {
259+
/* new entry; seconds since intial epoch */
260+
dso::FractionalSeconds sec =
261+
block_tai.diff<dso::DateTimeDifferenceType::FractionalSeconds>(
262+
start_t);
263+
/* integrate from initial conditions to this epoch */
264+
if (dop853.integrate(dso::MjdEpoch(start_t), sec.seconds(), state, y)) {
265+
fprintf(stderr, "ERROR. Integration failed! sec is %.3f\n",
266+
sec.seconds());
267+
return 1;
268+
}
269+
/* transform integration results to ECEF */
270+
Eigen::VectorXd yt = y;
271+
y.segment<3>(0) = R * yt.segment<3>(0);
272+
y.segment<3>(3) = R * yt.segment<3>(3) + dRdt * yt.segment<3>(0);
273+
printf("%.12f %.6f %.6f %.6f %.9f %.9f %.9f %.6f %.6f %.6f %.9f %.9f "
274+
"%.9f\n",
275+
sec.seconds(),
276+
block.state[0] * 1e3, block.state[1] * 1e3, block.state[2] * 1e3,
277+
block.state[4] * 1e-1, block.state[5] * 1e-1,
278+
block.state[6] * 1e-1, y(0), y(1), y(2), y(3), y(4), y(5));
279+
}
271280
}
272281
++it;
273-
if (it >= 2000)
282+
if (it >= 100)
274283
break;
275284
}
276285

277-
//Eigen::VectorXd state = Eigen::Matrix<double, 6, 1>::Zero();
278-
//Eigen::VectorXd y = Eigen::Matrix<double, 6, 1>::Zero();
279-
//Eigen::Matrix<double, 3, 3> R, dRdt;
280-
//std::size_t it = 0;
281-
//dso::Sp3DataBlock block;
282-
//int sp3err = 0;
283-
//while (!sp3err) {
284-
// sp3err = sp3.get_next_data_block(sv, block);
285-
// if (sp3err > 0) {
286-
// printf("Something went wrong ....status = %3d\n", sp3err);
287-
// return 1;
288-
// }
289-
// bool position_ok = !block.flag.is_set(dso::Sp3Event::bad_abscent_position);
290-
// if (position_ok && (!sp3err)) {
291-
// auto block_tai = block.t;
292-
// if (!std::strcmp(sp3.time_sys(), "GPS")) {
293-
// block_tai = block_tai.gps2tai();
294-
// }
295-
// if (gcrf2ecef(dso::MjdEpoch(block_tai), eop, R, dRdt, fargs, eopr)) {
296-
// return 8;
297-
// }
298-
// if (!it) {
299-
// /* first state of satellite in file; transform to celestial and store
300-
// * as state and start_t. This is where we start integrating from.
301-
// */
302-
// state << block.state[0] * 1e3, block.state[1] * 1e3,
303-
// block.state[2] * 1e3, block.state[4] * 1e-1, block.state[5] * 1e-1,
304-
// block.state[6] * 1e-1;
305-
// y.segment<3>(0) = R.transpose() * state.segment<3>(0);
306-
// y.segment<3>(3) = R.transpose() * state.segment<3>(3) +
307-
// dRdt.transpose() * state.segment<3>(0);
308-
// start_t = block_tai;
309-
// state = y;
310-
// } else {
311-
// /* new entry; seconds since intial epoch */
312-
// dso::FractionalSeconds sec =
313-
// block_tai.diff<dso::DateTimeDifferenceType::FractionalSeconds>(
314-
// start_t);
315-
// /* integrate from initial conditions to this epoch */
316-
// if (dop853.integrate(dso::MjdEpoch(start_t), sec.seconds(), state, y)) {
317-
// fprintf(stderr, "ERROR. Integration failed! sec is %.3f\n",
318-
// sec.seconds());
319-
// return 1;
320-
// }
321-
// /* transform integration results to ECEF */
322-
// Eigen::VectorXd yt = y;
323-
// y.segment<3>(0) = R * yt.segment<3>(0);
324-
// y.segment<3>(3) = R * yt.segment<3>(3) + dRdt * yt.segment<3>(0);
325-
// printf("%.12f %.6f %.6f %.6f %.9f %.9f %.9f %.6f %.6f %.6f %.9f %.9f "
326-
// "%.9f\n",
327-
// sec.seconds(),
328-
// block.state[0] * 1e3, block.state[1] * 1e3, block.state[2] * 1e3,
329-
// block.state[4] * 1e-1, block.state[5] * 1e-1,
330-
// block.state[6] * 1e-1, y(0), y(1), y(2), y(3), y(4), y(5));
331-
// }
332-
// }
333-
// ++it;
334-
// if (it >= 100)
335-
// break;
336-
//}
337-
338286
return sp3err;
339287
}

0 commit comments

Comments
 (0)