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