Skip to content

Commit 540561d

Browse files
xanthospapxanthospap
committed
sun not working
1 parent 64c9781 commit 540561d

File tree

2 files changed

+112
-31
lines changed

2 files changed

+112
-31
lines changed

‎bin/equations_of_motion.cpp‎

Lines changed: 51 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -10,14 +10,16 @@
1010
#include "yaml-cpp/yaml.h"
1111
#include <cstdio>
1212

13-
constexpr const double GM_Moon = 0.49028010560e13;
14-
constexpr const double GM_Sun = 1.32712442076e20;
13+
constexpr const double GM_Moon = /*0.49028010560e13;*/4902.800076e9;
14+
constexpr const double GM_Sun = /*1.32712442076e20;*/132712440040.944e9;
1515

1616
int gcrf2ecef(const dso::MjdEpoch &tai, dso::EopSeries &eops,
1717
Eigen::Matrix<double, 3, 3> &R,
18-
Eigen::Matrix<double, 3, 3> &dRdt) noexcept {
19-
double fargs[14];
20-
dso::EopRecord eopr;
18+
Eigen::Matrix<double, 3, 3> &dRdt,
19+
double *fargs,
20+
dso::EopRecord &eopr) noexcept {
21+
// double fargs[14];
22+
// dso::EopRecord eopr;
2123
// const auto tt = tai.tai2tt();
2224
const auto tt = tai.gps2tai().tai2tt();
2325
double X, Y;
@@ -71,9 +73,13 @@ int deriv(double tsec, Eigen::Ref<const Eigen::VectorXd> y0,
7173
dso::MjdEpoch t = params->t0();
7274
t.add_seconds(dso::FractionalSeconds(tsec));
7375

76+
/* Dealunay args (14) */
77+
double fargs[14];
78+
7479
/* Celestial to Terrestrial Matrix */
80+
dso::EopRecord eopr;
7581
Eigen::Matrix<double, 3, 3> R, dRdt;
76-
if (gcrf2ecef(dso::MjdEpoch(t), *(params->eops()), R, dRdt)) {
82+
if (gcrf2ecef(dso::MjdEpoch(t), *(params->eops()), R, dRdt, fargs, eopr)) {
7783
return 8;
7884
}
7985

@@ -85,39 +91,44 @@ int deriv(double tsec, Eigen::Ref<const Eigen::VectorXd> y0,
8591
const Eigen::VectorXd r_ecef = R * y0.segment<3>(0);
8692
Eigen::Matrix<double, 3, 1> acc;
8793
if (dso::sh2gradient_cunningham(stokes, r_ecef, acc, g,
88-
//stokes.max_degree(), stokes.max_order(), -1,
89-
80,80,-1,
94+
stokes.max_degree(), stokes.max_order(), -1,
9095
-1, &(params->tw()), &(params->tm()))) {
9196
fprintf(stderr, "ERROR Failed computing acceleration/gradient\n");
9297
return 1;
9398
}
9499

95100
// TODO adding sun worsens results !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
96-
Eigen::Matrix<double, 3, 1> acc_tb;
97-
{
98-
/* get Sun position in ICRF */
99-
Eigen::Matrix<double, 3, 1> rtb_sun;
100-
if (dso::planet_pos(dso::Planet::SUN, t.tai2tt(), rtb_sun)) {
101-
fprintf(stderr, "ERROR Failed to compute Sun position!\n");
102-
return 2;
103-
}
104-
acc_tb = dso::point_mass_acceleration(y0.segment<3>(0), rtb_sun, GM_Sun, g);
105-
/* get Moon position in ICRF */
106-
Eigen::Matrix<double, 3, 1> rtb_moon;
107-
if (dso::planet_pos(dso::Planet::MOON, t.tai2tt(), rtb_moon)) {
108-
fprintf(stderr, "ERROR Failed to compute Moon position!\n");
109-
return 2;
110-
}
111-
acc_tb +=
112-
dso::point_mass_acceleration(y0.segment<3>(0), rtb_moon, GM_Moon, g);
101+
Eigen::Matrix<double, 3, 1> acc_moon;
102+
Eigen::Matrix<double, 3, 1> acc_sun;
103+
/* get Sun position in ICRF */
104+
Eigen::Matrix<double, 3, 1> rtb_sun;
105+
if (dso::planet_pos(dso::Planet::SUN, t.tai2tt(), rtb_sun)) {
106+
fprintf(stderr, "ERROR Failed to compute Sun position!\n");
107+
return 2;
108+
}
109+
acc_sun = dso::point_mass_acceleration(y0.segment<3>(0), rtb_sun, GM_Sun);
110+
111+
/* get Moon position in ICRF */
112+
Eigen::Matrix<double, 3, 1> rtb_moon;
113+
if (dso::planet_pos(dso::Planet::MOON, t.tai2tt(), rtb_moon)) {
114+
fprintf(stderr, "ERROR Failed to compute Moon position!\n");
115+
return 2;
113116
}
117+
acc_moon = dso::point_mass_acceleration(y0.segment<3>(0), rtb_moon, GM_Moon);
118+
119+
/* Solid Earth Tide */
120+
params->mse_tide->stokes_coeffs(t.tai2tt(), t.tai2ut1(eopr.dut()), rtb_moon,
121+
rtb_sun, fargs);
114122

115123
/* set velocity vector (ICRF) */
116124
y.segment<3>(0) = y0.segment<3>(3);
117125

118126
/* ECEF to ICRF note that y = (v, a) and y0 = (r, v) */
119-
y.segment<3>(3) =
120-
R.transpose() * acc + acc_tb;
127+
//y.segment<3>(3) =
128+
// R.transpose() * acc + acc_tb;
129+
y.segment<3>(3) = (acc_moon + acc_sun);
130+
y.segment<3>(3) += R.transpose() * acc;
131+
121132

122133
return 0;
123134
}
@@ -187,6 +198,9 @@ int main(int argc, char *argv[]) {
187198
assert(stokes.max_order() == ORDER);
188199
}
189200

201+
/* Solid Earth Tides */
202+
dso::SolidEarthTide setide(iers2010::GMe, iers2010::Re, GM_Sun, GM_Moon);
203+
190204
/* setup integration parameters */
191205
dso::IntegrationParameters params;
192206
params.meops = &eop;
@@ -251,9 +265,18 @@ int main(int argc, char *argv[]) {
251265
}
252266
}
253267
++it;
254-
if (it >= 1000)
268+
if (it >= 100)
255269
break;
256270
}
257271

272+
int n;
273+
double GMSun, GMMoon;
274+
bodvrd_c("SUN", "GM", 1, &n, &GMSun);
275+
bodvrd_c("MOON", "GM", 1, &n, &GMMoon);
276+
printf("Note Moon GM=%.15e\n", GM_Moon);
277+
printf("CSPICE GM=%.15e\n", GMMoon);
278+
printf("Note Sun GM=%.15e\n", GM_Sun);
279+
printf("CSPICE GM=%.15e\n", GMSun);
280+
258281
return sp3err;
259282
}

‎bin/plot_integration_results.py‎

Lines changed: 61 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -60,6 +60,14 @@
6060
required=False,
6161
help='Plot state results and differences (same plot).')
6262

63+
parser.add_argument(
64+
'--compare-to',
65+
metavar='COMPARE_TO_FILE',
66+
dest='compare_to',
67+
required=False,
68+
default=None,
69+
help='Plot results versus a previous run.')
70+
6371
#parser.add_argument(
6472
# '--plot-norm',
6573
# action='store_true',
@@ -73,6 +81,26 @@
7381
# dest='quiet',
7482
# help='Do not show plot(s)')
7583

84+
def loadfn(fn):
85+
t = []; lx1=[]; ly1=[]; lz1=[];
86+
lx2=[]; ly2=[]; lz2=[];
87+
lvx1=[]; lvy1=[]; lvz1=[];
88+
lvx2=[]; lvy2=[]; lvz2=[];
89+
90+
with open(fn, 'r') as fin:
91+
for line in fin.readlines():
92+
if line[0] != '#':
93+
try:
94+
sec, x1, y1, z1, vx1, vy1, vz1, x2, y2, z2, vx2, vy2, vz2 = [ float(x) for x in line.split(' ') ]
95+
t.append(sec)
96+
lx1.append(x1); ly1.append(y1); lz1.append(z1);
97+
lx2.append(x2); ly2.append(y2); lz2.append(z2);
98+
lvx1.append(vx1); lvy1.append(vy1); lvz1.append(vz1);
99+
lvx2.append(vx2); lvy2.append(vy2); lvz2.append(vz2);
100+
except:
101+
pass
102+
return t, lx1, ly1, lz1, lvx1, lvy1, lvz1, lx2, ly2, lz2, lvx2, lvy2, lvz2
103+
76104
if __name__ == "__main__":
77105
args = parser.parse_args()
78106
scale_pos = args.scale_pos
@@ -82,6 +110,8 @@
82110
lx2=[]; ly2=[]; lz2=[];
83111
lvx1=[]; lvy1=[]; lvz1=[];
84112
lvx2=[]; lvy2=[]; lvz2=[];
113+
mean_height=0e0; mean_velo=0e0;
114+
N = 0
85115

86116
for line in sys.stdin:
87117
if line[0] != '#':
@@ -92,6 +122,9 @@
92122
lx2.append(x2); ly2.append(y2); lz2.append(z2);
93123
lvx1.append(vx1); lvy1.append(vy1); lvz1.append(vz1);
94124
lvx2.append(vx2); lvy2.append(vy2); lvz2.append(vz2);
125+
N += 1
126+
mean_height = mean_height * (N-1)/N + np.linalg.norm(np.array((x1,y1,z1))*1e-3)/N
127+
mean_velo = mean_velo * (N-1)/N + np.linalg.norm(np.array((vx1,vy1,vz1)))/N
95128
except:
96129
pass
97130
else:
@@ -105,14 +138,38 @@
105138
fig.subplots_adjust(hspace=0)
106139
group_labels = []
107140

141+
## Plot results compared to another file
142+
if args.compare_to is not None:
143+
st, slx1, sly1, slz1, slvx1, slvy1, slvz1, slx2, sly2, slz2, slvx2, slvy2, slvz2 = loadfn(args.compare_to)
144+
axs[0,0].plot(t, [scale_pos*(z[0]-z[1]) for z in zip(lx1,lx2)] ,label=r'$\delta x$ current')
145+
axs[1,0].plot(t, [scale_pos*(z[0]-z[1]) for z in zip(ly1,ly2)] ,label=r'$\delta y$ current')
146+
axs[2,0].plot(t, [scale_pos*(z[0]-z[1]) for z in zip(lz1,lz2)] ,label=r'$\delta z$ current')
147+
axs[0,1].plot(t, [scale_vel*(z[0]-z[1]) for z in zip(lvx1,lvx2)],label=r'$\delta v_x$ current')
148+
axs[1,1].plot(t, [scale_vel*(z[0]-z[1]) for z in zip(lvy1,lvy2)],label=r'$\delta v_y$ current')
149+
axs[2,1].plot(t, [scale_vel*(z[0]-z[1]) for z in zip(lvz1,lvz2)],label=r'$\delta v_z$ current')
150+
151+
axs[0,0].plot(st, [scale_pos*(z[0]-z[1]) for z in zip(slx1, slx2)] ,label=r'$\delta x$ {:}'.format(args.compare_to))
152+
axs[1,0].plot(st, [scale_pos*(z[0]-z[1]) for z in zip(sly1, sly2)] ,label=r'$\delta y$ {:}'.format(args.compare_to))
153+
axs[2,0].plot(st, [scale_pos*(z[0]-z[1]) for z in zip(slz1, slz2)] ,label=r'$\delta z$ {:}'.format(args.compare_to))
154+
axs[0,1].plot(st, [scale_vel*(z[0]-z[1]) for z in zip(slvx1,slvx2)],label=r'$\delta v_x$ {:}'.format(args.compare_to))
155+
axs[1,1].plot(st, [scale_vel*(z[0]-z[1]) for z in zip(slvy1,slvy2)],label=r'$\delta v_y$ {:}'.format(args.compare_to))
156+
axs[2,1].plot(st, [scale_vel*(z[0]-z[1]) for z in zip(slvz1,slvz2)],label=r'$\delta v_z$ {:}'.format(args.compare_to))
157+
158+
axs[0,0].legend(loc='upper left')
159+
axs[1,0].legend(loc='upper left')
160+
axs[2,0].legend(loc='upper left')
161+
axs[0,1].legend(loc='upper left')
162+
axs[1,1].legend(loc='upper left')
163+
axs[2,1].legend(loc='upper left')
164+
108165
## Plot state results (not differences)
109-
if (args.nodif or args.withdif):
166+
if (args.nodif or args.withdif) and not args.compare_to:
110167
print("Plotting state results ...")
111168
axs[0,0].plot(t, [scale_pos*z for z in lx1])
112169
axs[0,0].plot(t, [scale_pos*z for z in lx2])
113170
axs[0,1].plot(t, [scale_vel*z for z in lvx1])
114171
axs[0,1].plot(t, [scale_vel*z for z in lvx2])
115-
#
172+
#
116173
axs[1,0].plot(t, [scale_pos*z for z in ly1])
117174
axs[1,0].plot(t, [scale_pos*z for z in ly2])
118175
axs[1,1].plot(t, [scale_vel*z for z in lvy1])
@@ -137,7 +194,7 @@ def ytextpos(scale, list): return scale * sum(z for z in list[0:5]) / 5
137194
# group_labels.append("acc. group 2")
138195

139196
## Plot differences per component
140-
if (args.withdif or (not args.nodif)):
197+
if (args.withdif or (not args.nodif)) and not args.compare_to:
141198
print("Plotting state diffs ...")
142199
axs[0,0].plot(t, [scale_pos*(z[0]-z[1]) for z in zip(lx1,lx2)])
143200
axs[1,0].plot(t, [scale_pos*(z[0]-z[1]) for z in zip(ly1,ly2)])
@@ -172,6 +229,7 @@ def ytextpos(scale, list1, list2):
172229

173230
axs[2,0].set_xlabel("Sec of Integration (since t0) [sec]")
174231
axs[2,1].set_xlabel("Sec of Integration (since t0) [sec]")
232+
fig.suptitle('Mean Altitude is {:.1f}km, mean velocity is {:.1f}m/sec'.format(mean_height-6378., mean_velo), fontsize=16)
175233

176234
plt.show()
177235

0 commit comments

Comments
 (0)