def test_principal_radii():
lat = 0
re, rn = earth.principal_radii(lat)
assert_allclose(re, earth.R0, rtol=1e-10)
assert_allclose(rn, earth.R0 * (1 - earth.E2), rtol=1e-10)
lat = [0, 90]
re, rn = earth.principal_radii(lat)
assert_allclose(re[0], earth.R0, rtol=1e-10)
assert_allclose(rn[0], earth.R0 * (1 - earth.E2), rtol=1e-10)
assert_allclose(re[1], rn[1], rtol=1e-10)
def test_principal_radii():
# Really a smoke test of this formula implementation.
for model in ['WGS84', 'PZ90']:
earth.set_model(model)
slat = 0
re, rn = earth.principal_radii(slat)
assert_allclose(re, earth.R0, rtol=1e-10)
assert_allclose(rn, earth.R0 * (1 - earth.E2), rtol=1e-10)
slat = [0, 1]
re, rn = earth.principal_radii(slat)
assert_allclose(re[0], earth.R0, rtol=1e-10)
assert_allclose(rn[0], earth.R0 * (1 - earth.E2), rtol=1e-10)
assert_allclose(re[1], rn[1], rtol=1e-10)
def lla_to_ecef(lat, lon, alt=0):
"""Convert latitude, longitude, altitude to ECEF Cartesian coordinates.
Parameters
----------
lat, lon : array_like
Latitude and longitude.
alt : array_like, optional
Altitude. Default is 0.
Returns
-------
r_e : ndarray, shape (3,) or (n, 3)
Cartesian coordinates in ECEF frame.
"""
lat = np.asarray(lat)
sin_lat = np.sin(np.deg2rad(lat))
cos_lat = np.cos(np.deg2rad(lat))
sin_lon = np.sin(np.deg2rad(lon))
cos_lon = np.cos(np.deg2rad(lon))
re, _ = earth.principal_radii(lat)
r_e = np.empty((3,) + lat.shape)
r_e[0] = (re + alt) * cos_lat * cos_lon
r_e[1] = (re + alt) * cos_lat * sin_lon
r_e[2] = ((1 - earth.E2) * re + alt) * sin_lat
r_e = r_e.T
return r_e
def perturb_ll(lat, lon, d_lat, d_lon):
"""Perturb latitude and longitude.
This function recomputes linear displacements in meters to changes in a
latitude and longitude considering Earth curvature.
Note that this computation is approximate in nature and makes a good
sense only if displacements are significantly less than Earth radius.
Parameters
----------
lat, lon : array_like
Latitude and longitude.
d_lat, d_lon : array_like
Perturbations to `lat` and `lon` respectively in *meters*. It is
assumed that `d_lat` and `d_lon` are significantly less than Earth
radius.
Returns
-------
lat_new, lon_new
Perturbed values of latitude and longitude.
"""
slat = np.sin(np.deg2rad(lat))
clat = (1 - slat**2) ** 0.5
re, rn = earth.principal_radii(lat)
lat_new = lat + np.rad2deg(d_lat / rn)
lon_new = lon + np.rad2deg(d_lon / (re * clat))
return lat_new, lon_new
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
def align_wahba(dt, theta, dv, lat, VE=None, VN=None):
"""Estimate attitude matrix by solving Wahba's problem.
This method is based on solving a least-squares problem for a direction
cosine matrix A (originally formulated in [1]_)::
L = sum(||A r_i - b_i||^2, i=1, ..., m) -> min A,
s. t. A being a right orthogonal matrix.
Here ``(r_i, b_i)`` are measurements of the same unit vectors in two
frames.
The application of this method to self alignment of INS is explained in
[2]_. In this problem the vectors ``(r_i, b_i)`` are normalized velocity
increments due to gravity. It is applicable to dynamic conditions as well,
but in this case a full accuracy can be achieved only if velocity is
provided.
The optimization problem is solved using the most straightforward method
based on SVD [3]_.
Parameters
----------
dt : double
Sensors sampling period.
theta, dv : array_like, shape (n_samples, 3)
Rotation vectors and velocity increments computed from gyro and
accelerometer readings after applying coning and sculling
corrections.
lat : float
Latitude of the place.
VE, VN : array_like with shape (n_samples + 1, 3) or None
East and North velocity of the target. If None (default), it is
assumed to be 0. See Notes for further details.
Returns
-------
hpr : tuple of 3 floats
Estimated heading, pitch and roll at the end of the alignment.
P_align : ndarray, shape (3, 3)
Covariance matrix of misalignment angles, commonly known as
"phi-angle" in INS literature. Its values are measured in degrees
squared. This matrix is estimated in a rather ad-hoc fashion, see
Notes.
Notes
-----
If the alignment takes place in dynamic conditions but velocities `VE`
and `VN` are not provided, the alignment accuracy will be decreased (to
some extent it will be reflected in `P_align`). Note that `VE` and `VN` are
required with the same rate as inertial readings (and contain 1 more
sample). It means that you usually have to do some sort of interpolation.
In on-board implementation you just provide the last available velocity
data from GPS and it will work fine.
The paper [3]_ contains a recipe of computing the covariance matrix given
that errors in measurements are independent, small and follow a statistical
distribution with zero mean and known variance. In our case we estimate
measurement error variance from the optimal value of the optimized function
(see above). But as our errors are not independent and necessary small
(nor they follow any reasonable distribution) we don't scale their
variance by the number of observations (which is commonly done for the
variance of an average value). Some experiments show that this approach
gives reasonable values of `P_align`.
Also note, that `P_align` accounts only for misalignment errors due
to non-perfect alignment conditions. In addition to that, azimuth accuracy
is always limited by gyro drifts and level accuracy is limited by the
accelerometer biases. You should add these systematic uncertainties to the
diagonal of `P_align`.
References
----------
.. [1] G. Wahba, "Problem 65–1: A Least Squares Estimate of Spacecraft
Attitude", SIAM Review, 1965, 7(3), 409.
.. [2] P. M. G. Silson, "Coarse Alignment of a Ship’s Strapdown Inertial
Attitude Reference System Using Velocity Loci", IEEE Trans. Instrum.
Meas., vol. 60, pp. 1930-1941, Jun. 2011.
.. [3] F. L. Markley, "Attitude Determination using Vector Observations
and the Singular Value Decomposition", The Journal of the
Astronautical Sciences, Vol. 36, No. 3, pp. 245-258, Jul.-Sept.
1988.
"""
n_samples = theta.shape[0]
Vg = np.zeros((n_samples + 1, 3))
if VE is not None:
Vg[:, 0] = VE
if VN is not None:
Vg[:, 1] = VN
lat = np.deg2rad(lat)
slat, clat = np.sin(lat), np.cos(lat)
tlat = slat / clat
re, rn = earth.principal_radii(lat)
u = earth.RATE * np.array([0, clat, slat])
g = np.array([0, 0, -earth.gravity(slat)])
Cb0b = np.empty((n_samples + 1, 3, 3))
Cg0g = np.empty((n_samples + 1, 3, 3))
Cb0b[0] = np.identity(3)
Cg0g[0] = np.identity(3)
Vg_m = 0.5 * (Vg[1:] + Vg[:-1])
rho = np.empty_like(Vg_m)
rho[:, 0] = -Vg_m[:, 1] / rn
rho[:, 1] = Vg_m[:, 0] / re
rho[:, 2] = Vg_m[:, 0] / re * tlat
for i in range(n_samples):
Cg0g[i + 1] = Cg0g[i].dot(dcm.from_rv((rho[i] + u) * dt))
Cb0b[i + 1] = Cb0b[i].dot(dcm.from_rv(theta[i]))
f_g = np.cross(u, Vg) - g
f_g0 = util.mv_prod(Cg0g, f_g)
f_g0 = 0.5 * (f_g0[1:] + f_g0[:-1])
f_g0 = np.vstack((np.zeros(3), f_g0))
V_g0 = util.mv_prod(Cg0g, Vg) + dt * np.cumsum(f_g0, axis=0)
V_b0 = np.cumsum(util.mv_prod(Cb0b[:-1], dv), axis=0)
V_b0 = np.vstack((np.zeros(3), V_b0))
k = n_samples // 2
b = V_g0[k:2 * k] - V_g0[:k]
b /= np.linalg.norm(b, axis=1)[:, None]
r = V_b0[k:2 * k] - V_b0[:k]
r /= np.linalg.norm(r, axis=1)[:, None]
B = np.zeros((3, 3))
for bi, ri in zip(b, r):
B += np.outer(bi, ri)
n_obs = b.shape[0]
B /= n_obs
U, s, VT = svd(B, overwrite_a=True)
d = det(U) * det(VT)
Cg0b0 = U.dot(np.diag([1, 1, d])).dot(VT)
Cgb = Cg0g[-1].T.dot(Cg0b0).dot(Cb0b[-1])
s[-1] *= d
trace_s = np.sum(s)
L = 1 - trace_s
D = trace_s - s
M = np.identity(3) - np.diag(s)
if L < 0 or np.any(M < 0):
L = max(L, 0)
M[M < 0] = 0
warn("Negative values encountered when estimating the covariance, "
"they were set to zeros.")
R = (L * M / n_obs)**0.5 / D
R = U.dot(R)
R = Cg0g[-1].T.dot(R)
R = np.rad2deg(R)
return dcm.to_hpr(Cgb), R.dot(R.T)