def testFunction(self, bV, sV, lat, long, alt, time):
jdays = utc2jul.utc2jul(time)
sI = sun_vec.sun_vec(jdays - 2444244.5)
temp = np.matrix([0, 0, -1]).transpose()
temp = pm.ned2ecef(0, 0, -1, 45, 30, 0)
temp = temp / np.linalg.norm(temp)
print(temp)
print("sI:")
print(sI)
print("-----------------------------------------")
print()
gm = wrldmagm.wrldmagm("WMM2015.COF")
bI = np.matrix(gm.wrldmagm(lat, long, alt, decyear.decyear(time)))
bI = bI / np.linalg.norm(bI)
print("bI: (NED)")
print(bI)
print("-----------------------------------------")
print()
bI = np.squeeze(np.asarray(bI))
bI = pm.ned2ecef(bI[0], bI[1], bI[2], lat, long, alt)
bIECEF = np.matrix(bI).transpose()
bIECEF = bI / np.linalg.norm(bI)
print("bI: (ECEF)")
print(bIECEF)
print("-----------------------------------------")
print()
bI = pm.ecef2eci(bI, time)
bI = np.matrix(bI).transpose()
bI = bI / np.linalg.norm(bI)
print("bI: (ECI)")
print(bI)
print("------------------------------")
print()
print("DCM:")
return getDCM.getDCM(bV, sV, bI, sI)
def test_ned():
xyz = pm.aer2ecef(*aer0, *lla0)
enu = pm.aer2enu(*aer0)
ned = (enu[1], enu[0], -enu[2])
lla = pm.aer2geodetic(*aer0, *lla0)
assert pm.aer2ned(*aer0) == approx(ned0)
with pytest.raises(ValueError):
pm.aer2ned(aer0[0], aer0[1], -1)
assert pm.enu2aer(*enu) == approx(aer0)
assert pm.enu2aer(*enu, deg=False) == approx(raer0)
assert pm.ned2aer(*ned) == approx(aer0)
assert pm.ecef2ned(*xyz, *lla0) == approx(ned)
assert pm.ned2ecef(*ned, *lla0) == approx(xyz)
# %%
assert pm.ecef2enuv(vx, vy, vz, *lla0[:2]) == approx((ve, vn, vu))
assert pm.ecef2nedv(vx, vy, vz, *lla0[:2]) == approx((vn, ve, -vu))
# %%
enu3 = pm.geodetic2enu(*lla, *lla0)
ned3 = (enu3[1], enu3[0], -enu3[2])
assert pm.geodetic2ned(*lla, *lla0) == approx(ned3)
assert pm.enu2geodetic(*enu3, *lla0) == approx(lla)
assert pm.ned2geodetic(*ned3, *lla0) == approx(lla)
def test_ned():
xyz = pm.aer2ecef(*aer0, *lla0)
enu = pm.aer2enu(*aer0)
ned = (enu[1], enu[0], -enu[2])
lla = pm.aer2geodetic(*aer0, *lla0)
assert pm.aer2ned(*aer0) == approx(ned0)
with pytest.raises(ValueError):
pm.aer2ned(aer0[0], aer0[1], -1)
assert pm.enu2aer(*enu) == approx(aer0)
assert pm.enu2aer(*enu, deg=False) == approx(raer0)
assert pm.ned2aer(*ned) == approx(aer0)
assert pm.ecef2ned(*xyz, *lla0) == approx(ned)
assert pm.ned2ecef(*ned, *lla0) == approx(xyz)
# %%
assert pm.ecef2enuv(vx, vy, vz, *lla0[:2]) == approx((ve, vn, vu))
assert pm.ecef2nedv(vx, vy, vz, *lla0[:2]) == approx((vn, ve, -vu))
# %%
enu3 = pm.geodetic2enu(*lla, *lla0)
ned3 = (enu3[1], enu3[0], -enu3[2])
assert pm.geodetic2ned(*lla, *lla0) == approx(ned3)
assert pm.enu2geodetic(*enu3, *lla0) == approx(lla)
assert pm.ned2geodetic(*ned3, *lla0) == approx(lla)
def test_ecef_ned():
enu = pm.aer2enu(*aer0)
ned = (enu[1], enu[0], -enu[2])
xyz = pm.aer2ecef(*aer0, *lla0)
n, e, d = pm.ecef2ned(*xyz, *lla0)
assert n == approx(ned[0])
assert e == approx(ned[1])
assert d == approx(ned[2])
assert pm.ned2ecef(*ned, *lla0) == approx(xyz)
def space_carve_view(self, array_view, main_model, perturbations):
"""Do the space carving of a view on a separate process."""
logging.info("Space Carving: Processing camera {}.".format(
array_view.server_id))
#
# Collect cloud weights from participating cameras.
#
server_id = array_view.server_id
array_model = main_model.arrays.array_items[server_id]
#
# Get the masks.
#
manual_mask = array_view.image_widget.mask
joint_mask = (manual_mask * array_model.sunshader_mask).astype(
np.uint8)
#
# Get the masked grid scores for the specific camera.
#
mask_inds = joint_mask == 1
grid_score = np.ones_like(array_model.cloud_weights)
grid_score[mask_inds] = array_model.cloud_weights[mask_inds]
weights_shape = array_model.cloud_weights.shape
grid_scores_tmp = []
grid_masks_tmp = []
cloud_rgb_tmp = []
for _ in range(perturbations):
X, Y, Z = np.meshgrid(*main_model.GRID_NED)
X = X + main_model.delx * np.random.random(size=X.shape)
Y = Y + main_model.dely * np.random.random(size=Y.shape)
Z = Z - main_model.delz * np.random.random(size=Z.shape)
grid_3D = pymap3d.ned2ecef(X, Y, Z, main_model.latitude,
main_model.longitude,
main_model.altitude)
grid_2D = projectGrid(array_model, grid_3D, weights_shape)
grid_scores_tmp.append(grid_score[grid_2D[:, 0], grid_2D[:, 1]])
grid_masks_tmp.append(joint_mask[grid_2D[:, 0], grid_2D[:, 1]])
#
# Collect RGB values at grid points.
#
cloud_rgb_tmp.append(array_model.img_array[grid_2D[:, 0],
grid_2D[:, 1], ...])
return np.mean(grid_scores_tmp, axis=0), \
np.mean(grid_masks_tmp, axis=0), \
np.mean(cloud_rgb_tmp, axis=0)
def test_ned2ecef():
ned_pos = test_ecef2ned()
obs_ecef = my_blh2ecef(lat_o, lon_o, hig_o)
result_ecef = my_ned2ecef(ned_pos[0], ned_pos[1], ned_pos[2],
obs_ecef[0], obs_ecef[1], obs_ecef[2])
answer_ecef = pm.ned2ecef(ned_pos[0], ned_pos[1], ned_pos[2],
lat_o, lon_o, hig_o)
print("result_ecef : ", result_ecef)
print("answer_ecef : ", answer_ecef)
diff = [abs(j - i) for (i, j) in zip(result_ecef, answer_ecef)]
print("ecef diff : ", diff)
return
def test_ecefenu():
assert_allclose(pm.aer2ecef(taz, tel, tsrange, tlat, tlon, talt),
(a2x, a2y, a2z),
rtol=0.01,
err_msg='aer2ecef: {}'.format(
pm.aer2ecef(taz, tel, tsrange, tlat, tlon, talt)))
assert_allclose(pm.aer2enu(taz, tel, tsrange), (a2e, a2n, a2u),
rtol=0.01,
err_msg='aer2enu: ' + str(pm.aer2enu(taz, tel, tsrange)))
assert_allclose(pm.aer2ned(taz, tel, tsrange), (a2n, a2e, -a2u),
rtol=0.01,
err_msg='aer2ned: ' + str(pm.aer2ned(taz, tel, tsrange)))
assert_allclose(pm.ecef2enu(tx, ty, tz, tlat, tlon, talt), (e2e, e2n, e2u),
rtol=0.01,
err_msg='ecef2enu: {}'.format(
pm.ecef2enu(tx, ty, tz, tlat, tlon, talt)))
assert_allclose(pm.ecef2enuv(vx, vy, vz, tlat, tlon), (ve, vn, vu))
assert_allclose(pm.ecef2ned(tx, ty, tz, tlat, tlon, talt),
(e2n, e2e, -e2u),
rtol=0.01,
err_msg='ecef2ned: {}'.format(
pm.ecef2enu(tx, ty, tz, tlat, tlon, talt)))
assert_allclose(pm.ecef2nedv(vx, vy, vz, tlat, tlon), (vn, ve, -vu))
assert_allclose(pm.aer2geodetic(taz, tel, tsrange, tlat, tlon, talt),
(a2la, a2lo, a2a),
rtol=0.01,
err_msg='aer2geodetic {}'.format(
pm.aer2geodetic(taz, tel, tsrange, tlat, tlon, talt)))
assert_allclose(pm.ecef2aer(tx, ty, tz, tlat, tlon, talt),
(ec2az, ec2el, ec2rn),
rtol=0.01,
err_msg='ecef2aer {}'.format(
pm.ecef2aer(a2x, a2y, a2z, tlat, tlon, talt)))
#%%
assert_allclose(pm.enu2aer(te, tn, tu), (e2az, e2el, e2rn),
rtol=0.01,
err_msg='enu2aer: ' + str(pm.enu2aer(te, tn, tu)))
assert_allclose(pm.ned2aer(tn, te, -tu), (e2az, e2el, e2rn),
rtol=0.01,
err_msg='enu2aer: ' + str(pm.enu2aer(te, tn, tu)))
assert_allclose(pm.enu2geodetic(te, tn, tu, tlat, tlon,
talt), (lat2, lon2, alt2),
rtol=0.01,
err_msg='enu2geodetic: ' +
str(pm.enu2geodetic(te, tn, tu, tlat, tlon, talt)))
assert_allclose(pm.ned2geodetic(tn, te, -tu, tlat, tlon,
talt), (lat2, lon2, alt2),
rtol=0.01,
err_msg='enu2geodetic: ' +
str(pm.enu2geodetic(te, tn, tu, tlat, tlon, talt)))
assert_allclose(pm.enu2ecef(te, tn, tu, tlat, tlon, talt), (e2x, e2y, e2z),
rtol=0.01,
err_msg='enu2ecef: ' +
str(pm.enu2ecef(te, tn, tu, tlat, tlon, talt)))
assert_allclose(
pm.ned2ecef(tn, te, -tu, tlat, tlon, talt), (e2x, e2y, e2z),
rtol=0.01,
err_msg='ned2ecef: ' + str(pm.ned2ecef(tn, te, -tu, tlat, tlon, talt)))
def test_geodetic(self):
if pyproj:
ecef = pyproj.Proj(proj='geocent', ellps='WGS84', datum='WGS84')
lla = pyproj.Proj(proj='latlong', ellps='WGS84', datum='WGS84')
xyz1 = pm.geodetic2ecef(*lla0)
assert_allclose(pm.geodetic2ecef(*rlla0, deg=False),
xyz1,
err_msg='geodetic2ecef: rad')
assert_allclose(xyz1, xyz0, err_msg='geodetic2ecef: deg')
assert_allclose(pm.ecef2geodetic(*xyz1),
lla0,
err_msg='ecef2geodetic: deg')
assert_allclose(pm.ecef2geodetic(*xyz1, deg=False),
rlla0,
err_msg='ecef2geodetic: rad')
if pyproj:
assert_allclose(
pyproj.transform(lla, ecef, lla0[1], lla0[0], lla0[2]), xyz1)
assert_allclose(pyproj.transform(ecef, lla, *xyz1),
(lla0[1], lla0[0], lla0[2]))
lla2 = pm.aer2geodetic(*aer0, *lla0)
rlla2 = pm.aer2geodetic(*raer0, *rlla0, deg=False)
assert_allclose(lla2, lla1, err_msg='aer2geodetic: deg')
assert_allclose(rlla2, rlla1, err_msg='aer2geodetic:rad')
assert_allclose(pm.geodetic2aer(*lla2, *lla0),
aer0,
err_msg='geodetic2aer: deg')
assert_allclose(pm.geodetic2aer(*rlla2, *rlla0, deg=False),
raer0,
err_msg='geodetic2aer: rad')
# %% aer-ecef
xyz2 = pm.aer2ecef(*aer0, *lla0)
assert_allclose(pm.aer2ecef(*raer0, *rlla0, deg=False),
axyz0,
err_msg='aer2ecef:rad')
assert_allclose(xyz2, axyz0, err_msg='aer2ecef: deg')
assert_allclose(pm.ecef2aer(*xyz2, *lla0),
aer0,
err_msg='ecef2aer:deg')
assert_allclose(pm.ecef2aer(*xyz2, *rlla0, deg=False),
raer0,
err_msg='ecef2aer:rad')
# %% aer-enu
enu1 = pm.aer2enu(*aer0)
ned1 = (enu1[1], enu1[0], -enu1[2])
assert_allclose(enu1, enu0, err_msg='aer2enu: deg')
assert_allclose(pm.aer2enu(*raer0, deg=False),
enu0,
err_msg='aer2enu: rad')
assert_allclose(pm.aer2ned(*aer0), ned0, err_msg='aer2ned')
assert_allclose(pm.enu2aer(*enu1), aer0, err_msg='enu2aer: deg')
assert_allclose(pm.enu2aer(*enu1, deg=False),
raer0,
err_msg='enu2aer: rad')
assert_allclose(pm.ned2aer(*ned1), aer0, err_msg='ned2aer')
# %% enu-ecef
assert_allclose(pm.enu2ecef(*enu1, *lla0),
xyz2,
err_msg='enu2ecef: deg')
assert_allclose(pm.enu2ecef(*enu1, *rlla0, deg=False),
xyz2,
err_msg='enu2ecef: rad')
assert_allclose(pm.ecef2enu(*xyz2, *lla0),
enu1,
err_msg='ecef2enu:deg')
assert_allclose(pm.ecef2enu(*xyz2, *rlla0, deg=False),
enu1,
err_msg='ecef2enu:rad')
assert_allclose(pm.ecef2ned(*xyz2, *lla0), ned1, err_msg='ecef2ned')
assert_allclose(pm.ned2ecef(*ned1, *lla0), xyz2, err_msg='ned2ecef')
# %%
assert_allclose(pm.ecef2enuv(vx, vy, vz, *lla0[:2]), (ve, vn, vu))
assert_allclose(pm.ecef2nedv(vx, vy, vz, *lla0[:2]), (vn, ve, -vu))
# %%
enu3 = pm.geodetic2enu(*lla2, *lla0)
ned3 = (enu3[1], enu3[0], -enu3[2])
assert_allclose(pm.geodetic2ned(*lla2, *lla0),
ned3,
err_msg='geodetic2ned: deg')
assert_allclose(pm.enu2geodetic(*enu3, *lla0),
lla2,
err_msg='enu2geodetic')
assert_allclose(pm.ned2geodetic(*ned3, *lla0),
lla2,
err_msg='ned2geodetic')
def test_geodetic(self):
if pyproj:
ecef = pyproj.Proj(proj='geocent', ellps='WGS84', datum='WGS84')
lla = pyproj.Proj(proj='latlong', ellps='WGS84', datum='WGS84')
x1, y1, z1 = pm.geodetic2ecef(tlat, tlon, talt)
assert_allclose(
pm.geodetic2ecef(radians(tlat), radians(tlon), talt, deg=False),
(x1, y1, z1))
assert_allclose((x1, y1, z1), (x0, y0, z0), err_msg='geodetic2ecef')
assert_allclose(pm.ecef2geodetic(x1, y1, z1), (tlat, tlon, talt),
err_msg='ecef2geodetic')
if pyproj:
assert_allclose(pyproj.transform(lla, ecef, tlon, tlat, talt),
(x1, y1, z1))
assert_allclose(pyproj.transform(ecef, lla, x1, y1, z1),
(tlon, tlat, talt))
lat2, lon2, alt2 = pm.aer2geodetic(taz, tel, tsrange, tlat, tlon, talt)
assert_allclose((lat2, lon2, alt2), (lat1, lon1, alt1),
err_msg='aer2geodetic')
assert_allclose(pm.geodetic2aer(lat2, lon2, alt2, tlat, tlon, talt),
(taz, tel, tsrange),
err_msg='geodetic2aer')
x2, y2, z2 = pm.aer2ecef(taz, tel, tsrange, tlat, tlon, talt)
assert_allclose(
pm.aer2ecef(radians(taz),
radians(tel),
tsrange,
radians(tlat),
radians(tlon),
talt,
deg=False), (a2x, a2y, a2z))
assert_allclose((x2, y2, z2), (a2x, a2y, a2z), err_msg='aer2ecef')
assert_allclose(pm.ecef2aer(x2, y2, z2, tlat, tlon, talt),
(taz, tel, tsrange),
err_msg='ecef2aer')
e1, n1, u1 = pm.aer2enu(taz, tel, tsrange)
assert_allclose((e1, n1, u1), (e0, n0, u0), err_msg='aer2enu')
assert_allclose(pm.aer2ned(taz, tel, tsrange), (n0, e0, -u0),
err_msg='aer2ned')
assert_allclose(pm.enu2aer(e1, n1, u1), (taz, tel, tsrange),
err_msg='enu2aer')
assert_allclose(pm.ned2aer(n1, e1, -u1), (taz, tel, tsrange),
err_msg='ned2aer')
assert_allclose(pm.enu2ecef(e1, n1, u1, tlat, tlon, talt),
(x2, y2, z2),
err_msg='enu2ecef')
assert_allclose(pm.ecef2enu(x2, y2, z2, tlat, tlon, talt),
(e1, n1, u1),
err_msg='ecef2enu')
assert_allclose(pm.ecef2ned(x2, y2, z2, tlat, tlon, talt),
(n1, e1, -u1),
err_msg='ecef2ned')
assert_allclose(pm.ned2ecef(n1, e1, -u1, tlat, tlon, talt),
(x2, y2, z2),
err_msg='ned2ecef')
# %%
assert_allclose(pm.ecef2enuv(vx, vy, vz, tlat, tlon), (ve, vn, vu))
assert_allclose(pm.ecef2nedv(vx, vy, vz, tlat, tlon), (vn, ve, -vu))
#%%
e3, n3, u3 = pm.geodetic2enu(lat2, lon2, alt2, tlat, tlon, talt)
assert_allclose(pm.geodetic2ned(lat2, lon2, alt2, tlat, tlon, talt),
(n3, e3, -u3))
assert_allclose(pm.enu2geodetic(e3, n3, u3, tlat, tlon, talt),
(lat2, lon2, alt2),
err_msg='enu2geodetic')
assert_allclose(pm.ned2geodetic(n3, e3, -u3, tlat, tlon, talt),
(lat2, lon2, alt2),
err_msg='ned2geodetic')
vel_comp = df['IMU_ATTI(0):velComposite[meters/Sec]'].to_numpy()
print(vel_n.shape)
# import gps lat, lon, h
gps_lat = df['GPS(0):Lat[degrees]'].to_numpy()
gps_lon = df['GPS(0):Long[degrees]'].to_numpy()
baro_h = df['IMU_ATTI(0):barometer:Raw[meters]'].to_numpy()
gps_h = df['IMU_ATTI(0):barometer:Smooth[meters]'].to_numpy().copy()
vel_ecef = np.zeros((N, 3))
for ii in range(N):
vel_ecef[ii] = pymap3d.ned2ecef(n=vel_n[ii],
e=vel_e[ii],
d=vel_d[ii],
lat0=gps_lat[0],
lon0=gps_lon[0],
h0=baro_h[0],
deg=True)
vel_ecef -= vel_ecef[0]
vel_ecef_comp = np.linalg.norm(vel_ecef, axis=1)
# parse flight 1 data
f1_initial = np.argwhere(time_gps[:, 1] == 594416.0).item(0)
f1_final = np.argwhere(time_gps[:, 1] == 594561.0).item(0)
gps_h -= gps_h[f1_initial]
baro_h -= (baro_h[f1_initial] + 0.95
) # subtract initial baro plus arbitrary offset
print(f1_initial, f1_final)
# save to file
def solve_trajectory(rocket):
# Initial Attitude #############################
quat0 = coord.euler2quat(rocket.azimuth0, rocket.elevation0)
DCM_LAUNCHER2NED = coord.DCM_NED2BODY_quat(quat0).transpose()
################################################
# Initial Position #############################
pos0_ECEF = pm.geodetic2ecef(rocket.pos0_LLH[0], rocket.pos0_LLH[1],
rocket.pos0_LLH[2])
pos0_ECI = pm.ecef2eci(pos0_ECEF, rocket.launch_date)
pos0_NED = DCM_LAUNCHER2NED.dot(np.array([rocket.Lcg0, 0.0, 0.0]))
################################################
# onLauncher Trajectory ##################################################
# lower launch lugがランチャレール長を越した時点でランチクリア
# lower lugが抜けた時点での重心各値をメインのソルバへ渡す
time = np.arange(
0.0, 0.5 + np.sqrt(
2.0 * rocket.launcher_rail * rocket.m(0.0) / rocket.ref_thrust),
0.01)
x0 = np.zeros(13)
x0[0:3] = pos0_NED # Pos_NED
x0[3:6] = np.zeros((1, 3)) # Vel_NED
x0[6:9] = np.zeros((1, 3)) # omega
x0[9:13] = quat0 # quat
ode_log = odeint(onlauncher_dynamics,
x0,
time,
args=(rocket, DCM_LAUNCHER2NED.transpose()))
# onLauncher post ######
pos_LAUNCHER = np.array(
[DCM_LAUNCHER2NED.transpose().dot(pos) for pos in ode_log[:, 0:3]])
# ランチクリアまででカット
index_launch_clear = np.argmax(
rocket.launcher_rail <= pos_LAUNCHER[:, 0] - rocket.lower_lug)
# log
rocket.result.time_onlauncher_log = time[:index_launch_clear + 1]
rocket.result.pos_onlauncher_NED_log = ode_log[:index_launch_clear + 1,
0:3]
rocket.result.vel_onlauncher_NED_log = ode_log[:index_launch_clear + 1,
3:6]
rocket.result.omega_onlauncher_log = ode_log[:index_launch_clear + 1, 6:9]
rocket.result.quat_onlauncher_log = ode_log[:index_launch_clear + 1, 9:13]
pos_launch_clear_NED = rocket.result.pos_onlauncher_NED_log[-1, :]
vel_launch_clear_NED = rocket.result.vel_onlauncher_NED_log[-1, :]
omega_launch_clear = rocket.result.omega_onlauncher_log[-1, :]
quat_launch_clear = rocket.result.quat_onlauncher_log[-1, :]
time_launch_clear = rocket.result.time_onlauncher_log[-1]
altitude_launch_clear = -pos_launch_clear_NED[2]
DCM_NED2BODY = coord.DCM_NED2BODY_quat(quat_launch_clear)
# tipoff moment
if rocket.tipoff_exist:
mg_tipoff_NED = np.array([
0, 0,
rocket.m(time_launch_clear) * env.gravity(altitude_launch_clear)
])
mg_tipoff_BODY = DCM_NED2BODY.dot(mg_tipoff_NED)
moment_arm_tipoff = np.array(
[rocket.lower_lug - rocket.Lcg(time_launch_clear), 0, 0])
moment_tipoff = np.cross(mg_tipoff_BODY, moment_arm_tipoff)
Ij_pitch = rocket.Ij_pitch(
time_launch_clear) + rocket.m(time_launch_clear) * np.abs(
rocket.Lcg(time_launch_clear) - rocket.lower_lug)**2
Ij_roll = rocket.Ij_roll(time_launch_clear)
Ij = np.array([Ij_roll, Ij_pitch, Ij_pitch])
omegadot_tipoff = moment_tipoff / Ij # 下部ラグを支点とした機体質量でのチップオフ角加速度。あとで角速度に変換する
else:
omegadot_tipoff = 0.0
################################################
# Main Trajectory ##################################################
def estimate_end(total_impulse):
It = total_impulse
tf = 2.37 * It**0.37
tf_order = len(str(int(tf))) - 1
dt = 10.0**(tf_order - 4)
return 1.4 * tf, dt
if rocket.auto_end:
rocket.end_time, rocket.time_step = estimate_end(rocket.total_impulse)
start_time = time_launch_clear # + rocket.time_step
time = np.arange(start_time, rocket.end_time, rocket.time_step)
pos_launch_clear_ECEF = pm.ned2ecef(pos_launch_clear_NED[0],
pos_launch_clear_NED[1],
pos_launch_clear_NED[2],
rocket.pos0_LLH[0], rocket.pos0_LLH[1],
rocket.pos0_LLH[2])
pos_launch_clear_ECI = coord.DCM_ECI2ECEF(
time_launch_clear).transpose().dot(pos_launch_clear_ECEF)
DCM_NED2ECEF = coord.DCM_ECEF2NED(rocket.pos0_LLH).transpose()
DCM_ECI2ECEF = coord.DCM_ECI2ECEF(time_launch_clear)
x0 = np.zeros(13)
x0[0:3] = pos_launch_clear_ECI
x0[3:6] = coord.vel_ECEF2ECI(DCM_NED2ECEF.dot(vel_launch_clear_NED),
DCM_ECI2ECEF, pos_launch_clear_ECI)
x0[6:9] = omega_launch_clear + omegadot_tipoff * rocket.time_step
x0[9:13] = quat_launch_clear
ode_log = odeint(dynamics_odeint, x0, time, args=(rocket, ))
# Main post ######
date_array = rocket.launch_date + np.array(
[datetime.timedelta(seconds=sec) for sec in time])
pos_ECEF = np.array([
coord.DCM_ECI2ECEF(t).dot(pos)
for pos, t in zip(ode_log[:, 0:3], time)
])
pos_LLH = np.array(
[pm.ecef2geodetic(pos[0], pos[1], pos[2]) for pos in pos_ECEF])
# 着地後の分をカット
index_hard_landing = np.argmax(pos_LLH[:, 2] <= 0.0)
# log
rocket.result.time_log = time[:index_hard_landing + 1]
rocket.result.pos_ECI_log = ode_log[:index_hard_landing + 1, 0:3]
rocket.result.vel_ECI_log = ode_log[:index_hard_landing + 1, 3:6]
rocket.result.omega_log = ode_log[:index_hard_landing + 1, 6:9]
rocket.result.quat_log = ode_log[:index_hard_landing + 1, 9:13]
dynamics_result(rocket)
################################################
# Decent Trajectory ##################################################
if rocket.timer_mode:
index_apogee = np.argmax(rocket.result.time_log > rocket.t_1st)
else:
index_apogee = np.argmax(pos_LLH[:, 2])
time_apogee = rocket.result.time_log[index_apogee]
pos_apogee_ECI = rocket.result.pos_ECI_log[index_apogee]
vel_apogee_ECI = rocket.result.vel_ECI_log[index_apogee]
vel_apogee_ned = np.array([
0, 0,
coord.DCM_ECEF2NED(rocket.pos0_LLH).dot(
coord.vel_ECI2ECEF(vel_apogee_ECI, coord.DCM_ECI2ECEF(time_apogee),
pos_apogee_ECI))[2]
])
vel_apogee_ECI = coord.vel_ECEF2ECI(
coord.DCM_ECEF2NED(rocket.pos0_LLH).transpose().dot(vel_apogee_ned),
coord.DCM_ECI2ECEF(time_apogee), pos_apogee_ECI)
if rocket.payload_exist:
rocket.m_burnout -= rocket.payload.mass
est_landing_payload = pos_LLH[index_apogee, 2] / np.sqrt(
2.0 * rocket.payload.mass * env.gravity(pos_LLH[index_apogee, 2]) /
(env.get_std_density(0.0) * rocket.payload.CdS))
est_landing = pos_LLH[index_apogee, 2] / np.sqrt(
2.0 * rocket.result.m_log[index_apogee] *
env.gravity(pos_LLH[index_apogee, 2]) / (env.get_std_density(0.0) *
(rocket.CdS1 + rocket.CdS2)))
time = np.arange(time_apogee, time_apogee + 1.2 * est_landing, 0.1)
x0 = np.zeros(6)
x0[0:3] = pos_apogee_ECI
x0[3:6] = vel_apogee_ECI
ode_log = odeint(parachute_dynamics, x0, time, args=(rocket, ))
# Decent post ######
date_array = rocket.launch_date + np.array(
[datetime.timedelta(seconds=sec) for sec in time])
pos_ECEF = np.array([
coord.DCM_ECI2ECEF(t).dot(pos)
for pos, t in zip(ode_log[:, 0:3], time)
])
pos_LLH = np.array(
[pm.ecef2geodetic(pos[0], pos[1], pos[2]) for pos in pos_ECEF])
# 着地後の分をカット
index_soft_landing = np.argmax(pos_LLH[:, 2] <= 0.0)
# log
rocket.result.time_decent_log = time[:index_soft_landing + 1]
rocket.result.pos_decent_ECI_log = ode_log[:index_soft_landing + 1, 0:3]
rocket.result.vel_decent_ECI_log = ode_log[:index_soft_landing + 1, 3:6]
# Payload ####
if rocket.payload_exist:
time = np.arange(time_apogee, time_apogee + 1.2 * est_landing_payload,
0.01)
x0 = np.zeros(6)
x0[0:3] = pos_apogee_ECI
x0[3:6] = vel_apogee_ECI
ode_log = odeint(payload_parachute_dynamics, x0, time, args=(rocket, ))
date_array = rocket.launch_date + np.array(
[datetime.timedelta(seconds=sec) for sec in time])
pos_ECEF = np.array([
coord.DCM_ECI2ECEF(t).dot(pos)
for pos, t in zip(ode_log[:, 0:3], time)
])
pos_LLH = np.array(
[pm.ecef2geodetic(pos[0], pos[1], pos[2]) for pos in pos_ECEF])
# 着地後の分をカット
index_payload_landing = np.argmax(pos_LLH[:, 2] <= 0.0)
# log
rocket.payload.result.time_decent_log = time[:index_payload_landing +
1]
rocket.payload.result.pos_decent_LLH_log = pos_LLH[:
index_payload_landing
+ 1, :]
