Esempio n. 1
0
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)
Esempio n. 2
0
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
Esempio n. 3
0
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)
Esempio n. 4
0
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)
Esempio n. 5
0
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
Esempio n. 6
0
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