def gravitation_ecef(lat, lon, alt=0):
"""Compute a vector of the gravitational force in ECEF frame.
It accounts only for Earth mass attraction. It is computed from
`gravity` model by eliminating the centrifugal force.
Parameters
----------
lat, lon : array_like
Latitude and longitude.
alt : array_like, optional
Altitude. Default is 0.
Returns
-------
g0_e: ndarray, shape (3,) or (n, 3)
Vectors of the gravitational force expressed in ECEF frame.
"""
sin_lat = np.sin(np.deg2rad(lat))
cos_lat = np.cos(np.deg2rad(lat))
re, _ = principal_radii(lat)
rp = (re + alt) * cos_lat
g0_g = np.zeros((3, ) + sin_lat.shape)
g0_g[1] = RATE**2 * rp * sin_lat
g0_g[2] = -gravity(lat, alt) - RATE**2 * rp * cos_lat
g0_g = g0_g.T
Ceg = dcm.from_llw(lat, lon)
return mv_prod(Ceg, g0_g)
def stationary_rotation(dt, lat, alt, Cnb, Cbs=None):
"""Simulate readings on a stationary bench.
Parameters
----------
dt : float
Time step.
lat : float
Latitude of the place.
alt : float
Altitude of the place.
Cnb : ndarray, shape (n_points, 3, 3)
Body attitude matrix.
Cbs : ndarray with shape (3, 3) or (n_points, 3, 3) or None
Sensor assembly attitude matrix relative to the body axes. If None,
(default) identity attitude is assumed.
Returns
-------
gyro, accel : ndarray, shape (n_points - 1, 3)
Gyro and accelerometer readings.
"""
n_points = Cnb.shape[0]
time = dt * np.arange(n_points)
lon_inertial = np.rad2deg(earth.RATE) * time
lat = np.full_like(lon_inertial, lat)
Cin = dcm.from_llw(lat, lon_inertial)
R = transform.lla_to_ecef(lat, lon_inertial, alt)
v_s = CubicSpline(time, R).derivative()
if Cbs is None:
Cns = Cnb
else:
Cns = util.mm_prod(Cnb, Cbs)
Cis = util.mm_prod(Cin, Cns)
Cib_spline = RotationSpline(time, Rotation.from_matrix(Cis))
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(Cis[:-1], d, at=True)
e = util.mv_prod(Cis[:-1], e, at=True)
gyros, accels = _compute_readings(dt, a, b, c, d, e)
return gyros, accels
def test_from_llw():
llw1 = np.array([90, -90, 0])
A1 = np.identity(3)
assert_allclose(dcm.from_llw(*llw1), A1, rtol=1e-10, atol=1e-10)
assert_allclose(dcm.from_llw(*llw1[:2]), A1, rtol=1e-10, atol=1e-10)
llw2 = np.array([90, -90, np.rad2deg(1e-9)])
A2 = np.array([[1, -1e-9, 0], [1e-9, 1, 0], [0, 0, 1]])
assert_allclose(dcm.from_llw(*llw2), A2, rtol=1e-10, atol=1e-10)
llw3 = np.array([-30, -45, 90])
A3 = np.array([[np.sqrt(2) / 4, -np.sqrt(2) / 2,
np.sqrt(6) / 4],
[-np.sqrt(2) / 4, -np.sqrt(2) / 2, -np.sqrt(6) / 4],
[np.sqrt(3) / 2, 0, -0.5]])
assert_allclose(dcm.from_llw(*llw3), A3, rtol=1e-10, atol=1e-10)
A4 = np.array([[2**0.5 / 2, 2**0.5 / 4, 6**0.5 / 4],
[2**0.5 / 2, -2**0.5 / 4, -6**0.5 / 4],
[0, 3**0.5 / 2, -0.5]])
assert_allclose(dcm.from_llw(*llw3[:2]), A4, rtol=1e-10, atol=1e-10)
llw = np.empty((15, 3))
llw[:5] = llw1
llw[5:10] = llw2
llw[10:] = llw3
A = np.empty((15, 3, 3))
A[:5] = A1
A[5:10] = A2
A[10:] = A3
assert_allclose(dcm.from_llw(*llw.T), A, rtol=1e-10, atol=1e-10)
def test_dcm_llw_conversion():
rng = np.random.RandomState(0)
lat = rng.uniform(-90, 90, 20)
lon = rng.uniform(-180, 180, 20)
wan = rng.uniform(-180, 180, 20)
A = dcm.from_llw(lat, lon, wan)
lat_r, lon_r, wan_r = dcm.to_llw(A)
assert_allclose(lon_r, lon, rtol=1e-10)
assert_allclose(lat_r, lat, rtol=1e-10)
assert_allclose(wan_r, wan, rtol=1e-10)
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