def test_dcm_Spline():
ht = [0, 45, 90]
C = dcm.from_hpr(ht, 0, 0)
t = [0, 45, 90]
s = dcm.Spline(t, C)
t_test = [0, 30, 60, 90]
C_test = s(t_test)
h, p, r = dcm.to_hpr(C_test)
assert_allclose(h, [0, 30, 60, 90], rtol=1e-14, atol=1e-16)
assert_allclose(p, 0, atol=1e-16)
assert_allclose(r, 0, atol=1e-16)
omega = np.rad2deg(s(t_test, 1))
assert_allclose(omega[:, 0], 0, atol=1e-16)
assert_allclose(omega[:, 1], 0, atol=1e-6)
assert_allclose(omega[:, 2], -1)
beta = np.rad2deg(s(t_test, 2))
assert_allclose(beta, 0, atol=1e-16)
t = np.linspace(0, 100, 101)
ht = 10 * t + 5 * np.sin(2 * np.pi * t / 10)
pt = 7 * t + 3 * np.sin(2 * np.pi * t / 10 + 2)
rt = -3 * t + 3 * np.sin(2 * np.pi * t / 10 - 2)
C = dcm.from_hpr(ht, pt, rt)
s = dcm.Spline(t, C)
Cs = s(t[::-1])
assert_allclose(Cs[::-1], C)
def test_from_hpr():
hpr1 = [30, 0, 0]
A_true1 = np.array([[np.sqrt(3) / 2, 0.5, 0], [-0.5,
np.sqrt(3) / 2, 0],
[0, 0, 1]])
assert_allclose(dcm.from_hpr(*hpr1), A_true1, rtol=1e-10)
hpr2 = np.rad2deg([1e-10, 3e-10, -1e-10])
A_true2 = np.array([[1, 1e-10, -1e-10], [-1e-10, 1, -3e-10],
[1e-10, 3e-10, 1]])
assert_allclose(dcm.from_hpr(*hpr2), A_true2, rtol=1e-8)
hpr3 = [45, -30, 60]
A_true3 = np.array([[
-np.sqrt(6) / 8 + np.sqrt(2) / 4,
np.sqrt(6) / 4,
np.sqrt(2) / 8 + np.sqrt(6) / 4
],
[
-np.sqrt(2) / 4 - np.sqrt(6) / 8,
np.sqrt(6) / 4, -np.sqrt(6) / 4 + np.sqrt(2) / 8
], [-0.75, -0.5, np.sqrt(3) / 4]])
assert_allclose(dcm.from_hpr(*hpr3), A_true3, rtol=1e-8)
hpr = np.vstack((hpr1, hpr2, hpr3)).T
A = np.array((A_true1, A_true2, A_true3))
assert_allclose(dcm.from_hpr(*hpr), A, rtol=1e-8)
def __init__(self, dt, h0, p0, r0, rot_speed=10, rest_time=20):
self.dt = dt
self.rot_speed = rot_speed
self.rest_time = rest_time
self.Cnb = np.empty((1, 3, 3))
self.Cnb[0] = dcm.from_hpr(h0, p0, r0)
self._rest_intervals = []
def test_stationary():
dt = 0.1
n_points = 100
t = dt * np.arange(n_points)
lat = 58
alt = -10
h = np.full(n_points, 45)
p = np.full(n_points, -10)
r = np.full(n_points, 5)
Cnb = dcm.from_hpr(h, p, r)
slat = np.sin(np.deg2rad(lat))
clat = (1 - slat**2)**0.5
omega_n = earth.RATE * np.array([0, clat, slat])
g_n = np.array([0, 0, -earth.gravity(lat, alt)])
Omega_b = Cnb[0].T.dot(omega_n)
g_b = Cnb[0].T.dot(g_n)
gyro, accel = sim.stationary_rotation(0.1, lat, alt, Cnb)
gyro_true = np.tile(Omega_b * dt, (n_points - 1, 1))
accel_true = np.tile(-g_b * dt, (n_points - 1, 1))
assert_allclose(gyro, gyro_true)
assert_allclose(accel, accel_true, rtol=1e-5, atol=1e-8)
# Rotate around Earth's axis with additional rate.
rate = 6
rate_n = rate * np.array([0, clat, slat])
rate_s = Cnb[0].T.dot(rate_n)
Cbs = dcm.from_rv(rate_s * t[:, None])
gyro, accel = sim.stationary_rotation(0.1, lat, alt, Cnb, Cbs)
gyro_true = np.tile((Omega_b + rate_s) * dt, (n_points - 1, 1))
assert_allclose(gyro, gyro_true)
# Place IMU horizontally and rotate around vertical axis.
# Gravity components should be identically 0.
p = np.full(n_points, 0)
r = np.full(n_points, 0)
Cnb = dcm.from_hpr(h, p, r)
Cbs = dcm.from_hpr(rate * t, 0, 0)
gyro, accel = sim.stationary_rotation(0.1, lat, alt, Cnb, Cbs)
accel_true = np.tile(-g_n * dt, (n_points - 1, 1))
assert_allclose(accel, accel_true, rtol=1e-5, atol=1e-7)
def test_dcm_hpr_conversion():
rng = np.random.RandomState(0)
h = rng.uniform(0, 360, 20)
p = rng.uniform(-90, 90, 20)
r = rng.uniform(-180, 180, 20)
A = dcm.from_hpr(h, p, r)
h_r, p_r, r_r = dcm.to_hpr(A)
assert_allclose(h, h_r, rtol=1e-10)
assert_allclose(p, p_r, rtol=1e-10)
assert_allclose(r, r_r, rtol=1e-10)
def test_dcm_gibbs_conversion():
np.random.seed(1)
h = np.random.uniform(0, 360, 100)
p = np.random.uniform(-90, 90, 100)
r = np.random.uniform(-180, 180, 100)
As = dcm.from_hpr(h, p, r)
for A in As:
grp = dcm.to_gibbs(A)
Ac = dcm.from_gibbs(grp)
assert_allclose(Ac, A, rtol=1e-14, atol=1e-15)
grp = dcm.to_gibbs(As)
Asc = dcm.from_gibbs(grp)
assert_allclose(Asc, As, rtol=1e-14, atol=1e-15)
def test_dcm_quat_conversion():
np.random.seed(0)
h = np.random.uniform(0, 360, 20)
p = np.random.uniform(-90, 90, 20)
r = np.random.uniform(-180, 180, 20)
As = dcm.from_hpr(h, p, r)
for A in As:
q = dcm.to_quat(A)
Ac = dcm.from_quat(q)
assert_allclose(Ac, A, rtol=1e-14, atol=1e-16)
q = dcm.to_quat(As)
Asc = dcm.from_quat(q)
assert_allclose(Asc, As, rtol=1e-14, atol=1e-16)
def test_match_vectors():
Cab_true = dcm.from_hpr(20, -10, 5)
vb = np.array([[0, 1, 0], [0, 0, 1]])
va = vb.dot(Cab_true.T)
Cab = dcm.match_vectors(va, vb)
assert_allclose(Cab, Cab_true, atol=1e-16)
Cab = dcm.match_vectors(va, vb, [200, 1])
assert_allclose(Cab, Cab_true, atol=1e-16)
rng = np.random.RandomState(0)
vb = rng.rand(100, 3)
vb /= np.linalg.norm(vb, axis=1)[:, None]
va = vb.dot(Cab_true.T)
Cab = dcm.match_vectors(va, vb)
assert_allclose(Cab, Cab_true, atol=1e-16)
def test_integrate():
# Test on the static bench.
dt = 1e-1
n = 100
Cnb = dcm.from_hpr(45, -30, 60)
gyro = np.array([0, 1 / np.sqrt(2), 1 / np.sqrt(2)]) * earth.RATE * dt
gyro = Cnb.T.dot(gyro)
gyro = np.resize(gyro, (n, 3))
accel = np.array([0, 0, earth.gravity(1 / np.sqrt(2))]) * dt
accel = Cnb.T.dot(accel)
accel = np.resize(accel, (n, 3))
theta, dv = coning_sculling(gyro, accel)
traj = integrate(dt, 45, 50, 0, 0, 45, -30, 60, theta, dv)
assert_allclose(traj.lat, 45, rtol=1e-12)
assert_allclose(traj.lon, 50, rtol=1e-12)
assert_allclose(traj.VE, 0, atol=1e-8)
assert_allclose(traj.VN, 0, atol=1e-8)
assert_allclose(traj.h, 45, rtol=1e-12)
assert_allclose(traj.p, -30, rtol=1e-12)
assert_allclose(traj.r, 60, rtol=1e-12)
Integrator.INITIAL_SIZE = 50
I = Integrator(dt, 45, 50, 0, 0, 45, -30, 60)
I.integrate(theta[:n // 2], dv[:n // 2])
I.integrate(theta[n // 2:], dv[n // 2:])
assert_allclose(I.traj.lat, 45, rtol=1e-12)
assert_allclose(I.traj.lon, 50, rtol=1e-12)
assert_allclose(I.traj.VE, 0, atol=1e-8)
assert_allclose(I.traj.VN, 0, atol=1e-8)
assert_allclose(I.traj.h, 45, rtol=1e-12)
assert_allclose(I.traj.p, -30, rtol=1e-12)
assert_allclose(I.traj.r, 60, rtol=1e-12)
def test_wahba():
lat = 45
Cnb = dcm.from_hpr(45, -30, 60)
dt = 1e-1
n = 1000
gyro = np.array([0, 1 / np.sqrt(2), 1 / np.sqrt(2)]) * earth.RATE * dt
gyro = Cnb.T.dot(gyro)
gyro = np.resize(gyro, (n, 3))
accel = np.array([0, 0, earth.gravity(1 / np.sqrt(2))]) * dt
accel = Cnb.T.dot(accel)
accel = np.resize(accel, (n, 3))
np.random.seed(0)
gyro += 1e-6 * np.random.randn(*gyro.shape) * dt
accel += 1e-4 * np.random.randn(*accel.shape) * dt
phi, dv = coning_sculling(gyro, accel)
hpr, P = align.align_wahba(dt, phi, dv, lat)
assert_allclose(hpr, [45, -30, 60], rtol=1e-3)
def from_position(dt, lat, lon, alt, h, p, r):
"""Generate inertial readings given position and attitude.
Parameters
----------
dt : float
Time step.
lat, lon, alt : array_like, shape (n_points,)
Time series of latitude, longitude and altitude.
h, p, r : array_like, shape (n_points,)
Time series of heading, pitch and roll angles.
Returns
-------
traj : DataFrame
Trajectory. Contains n_points rows.
gyro : ndarray, shape (n_points - 1, 3)
Gyro readings.
accel : ndarray, shape (n_points - 1, 3)
Accelerometer readings.
"""
lat = np.asarray(lat, dtype=float)
lon = np.asarray(lon, dtype=float)
alt = np.asarray(alt, dtype=float)
h = np.asarray(h, dtype=float)
p = np.asarray(p, dtype=float)
r = np.asarray(r, dtype=float)
n_points = lat.shape[0]
time = dt * np.arange(n_points)
lat_inertial = lat.copy()
lon_inertial = lon.copy()
lon_inertial += np.rad2deg(earth.RATE) * time
Cin = dcm.from_llw(lat_inertial, lon_inertial)
R = transform.lla_to_ecef(lat_inertial, lon_inertial, alt)
v_s = CubicSpline(time, R).derivative()
v = v_s(time)
V = v.copy()
V[:, 0] += earth.RATE * R[:, 1]
V[:, 1] -= earth.RATE * R[:, 0]
V = util.mv_prod(Cin, V, at=True)
Cnb = dcm.from_hpr(h, p, r)
Cib = util.mm_prod(Cin, Cnb)
Cib_spline = RotationSpline(time, Rotation.from_matrix(Cib))
a = Cib_spline.interpolator.c[2]
b = Cib_spline.interpolator.c[1]
c = Cib_spline.interpolator.c[0]
g = earth.gravitation_ecef(lat_inertial, lon_inertial, alt)
a_s = v_s.derivative()
d = a_s.c[1] - g[:-1]
e = a_s.c[0] - np.diff(g, axis=0) / dt
d = util.mv_prod(Cib[:-1], d, at=True)
e = util.mv_prod(Cib[:-1], e, at=True)
gyros, accels = _compute_readings(dt, a, b, c, d, e)
traj = pd.DataFrame(index=np.arange(time.shape[0]))
traj['lat'] = lat
traj['lon'] = lon
traj['alt'] = alt
traj['VE'] = V[:, 0]
traj['VN'] = V[:, 1]
traj['VU'] = V[:, 2]
traj['h'] = h
traj['p'] = p
traj['r'] = r
return traj, gyros, accels
def from_velocity(dt, lat0, lon0, alt0, VE, VN, VU, h, p, r):
"""Generate inertial readings given velocity and attitude.
Parameters
----------
dt : float
Time step.
lat0, lon0, alt0 : float
Initial values of latitude, longitude and altitude.
VE, VN, VU : array_like, shape (n_points,)
Time series of East, North and vertical velocity components.
h, p, r : array_like, shape (n_points,)
Time series of heading, pitch and roll angles.
Returns
-------
traj : DataFrame
Trajectory. Contains n_points rows.
gyro : ndarray, shape (n_points - 1, 3)
Gyro readings.
accel : ndarray, shape (n_points - 1, 3)
Accelerometer readings.
"""
MAX_ITER = 3
ACCURACY = 0.01
VE = np.asarray(VE, dtype=float)
VN = np.asarray(VN, dtype=float)
VU = np.asarray(VU, dtype=float)
h = np.asarray(h, dtype=float)
p = np.asarray(p, dtype=float)
r = np.asarray(r, dtype=float)
n_points = VE.shape[0]
time = np.arange(n_points) * dt
VU_spline = _QuadraticSpline(time, VU)
alt_spline = VU_spline.antiderivative()
alt = alt0 + alt_spline(time)
lat0 = np.deg2rad(lat0)
lon0 = np.deg2rad(lon0)
lat = lat0
for iteration in range(MAX_ITER):
_, rn = earth.principal_radii(np.rad2deg(lat))
rn += alt
dlat_spline = _QuadraticSpline(time, VN / rn)
lat_spline = dlat_spline.antiderivative()
lat_new = lat_spline(time) + lat0
delta = (lat - lat_new) * rn
lat = lat_new
if np.all(np.abs(delta) < ACCURACY):
break
re, _ = earth.principal_radii(np.rad2deg(lat))
re += alt
dlon_spline = _QuadraticSpline(time, VE / (re * np.cos(lat)))
lon_spline = dlon_spline.antiderivative()
lon = lon_spline(time) + lon0
lat = np.rad2deg(lat)
lon = np.rad2deg(lon)
lon_inertial = lon + np.rad2deg(earth.RATE) * time
Cin = dcm.from_llw(lat, lon_inertial)
v = np.vstack((VE, VN, VU)).T
v = util.mv_prod(Cin, v)
R = transform.lla_to_ecef(lat, lon_inertial, alt)
v[:, 0] -= earth.RATE * R[:, 1]
v[:, 1] += earth.RATE * R[:, 0]
v_s = _QuadraticSpline(time, v)
Cnb = dcm.from_hpr(h, p, r)
Cib = util.mm_prod(Cin, Cnb)
Cib_spline = RotationSpline(time, Rotation.from_matrix(Cib))
a = Cib_spline.interpolator.c[2]
b = Cib_spline.interpolator.c[1]
c = Cib_spline.interpolator.c[0]
g = earth.gravitation_ecef(lat, lon_inertial, alt)
a_s = v_s.derivative()
d = a_s.c[1] - g[:-1]
e = a_s.c[0] - np.diff(g, axis=0) / dt
d = util.mv_prod(Cib[:-1], d, at=True)
e = util.mv_prod(Cib[:-1], e, at=True)
gyros, accels = _compute_readings(dt, a, b, c, d, e)
traj = pd.DataFrame(index=np.arange(n_points))
traj['lat'] = lat
traj['lon'] = lon
traj['alt'] = alt
traj['VE'] = VE
traj['VN'] = VN
traj['VU'] = VU
traj['h'] = h
traj['p'] = p
traj['r'] = r
return traj, gyros, accels