def interpolate(path, ti):
"""
Returns the interpolated point along the path
Parameters
----------
path: tuple of n-vectors (positionA, positionB)
ti: real scalar
interpolation time assuming position A and B is at t0=0 and t1=1,
respectively.
Returns
-------
point: Nvector
point of interpolation along path
Notes
-----
The result for spherical Earth is returned.
"""
n_EB_E_t0, n_EB_E_t1 = path
n_EB_E_ti = unit(n_EB_E_t0 + ti * (n_EB_E_t1 - n_EB_E_t0),
norm_zero_vector=np.nan)
return n_EB_E_ti
def test_Ex6_interpolated_position():
# Position B at time t0 and t2 is given as n_EB_E_t0 and n_EB_E_t1:
# Enter elements as lat/long in deg:
n_EB_E_t0 = lat_lon2n_E(rad(89), rad(0))
n_EB_E_t1 = lat_lon2n_E(rad(89), rad(180))
# The times are given as:
t0 = 10
t1 = 20
ti = 16 # time of interpolation
# Find the interpolated position at time ti, n_EB_E_ti
# SOLUTION:
# Using standard interpolation:
n_EB_E_ti = unit(n_EB_E_t0 + (ti - t0) * (n_EB_E_t1 - n_EB_E_t0) /
(t1 - t0))
# When displaying the resulting position for humans, it is more
# convenient to see lat, long:
lat_EB_ti, long_EB_ti = n_E2lat_lon(n_EB_E_ti)
msg = 'Ex6, Interpolated position: lat, long = {} {} deg'
print(msg.format(deg(lat_EB_ti), deg(long_EB_ti)))
assert_allclose(deg(lat_EB_ti), 89.7999805)
assert_allclose(deg(long_EB_ti), 180.)
def test_Ex10_cross_track_distance():
# Position A1 and A2 and B are given as n_EA1_E, n_EA2_E, and n_EB_E:
# Enter elements as lat/long in deg:
n_EA1_E = lat_lon2n_E(rad(0), rad(0))
n_EA2_E = lat_lon2n_E(rad(10), rad(0))
n_EB_E = lat_lon2n_E(rad(1), rad(0.1))
radius = 6371e3 # m, mean Earth radius
# Find the cross track distance from path A to position B.
# SOLUTION:
# Find the unit normal to the great circle:
c_E = unit(np.cross(n_EA1_E, n_EA2_E, axis=0))
# Find the great circle cross track distance:
sin_theta = -np.dot(c_E.T, n_EB_E) # pylint: disable=invalid-unary-operand-type
s_xt = np.arcsin(sin_theta) * radius
# ill conditioned for small angles:
# s_xt2 = (np.arccos(-sin_theta) - np.pi / 2) * radius
# Find the Euclidean cross track distance:
d_xt = sin_theta * radius
msg = 'Ex10, Cross track distance = {} m, Euclidean = {} m'
print(msg.format(s_xt, d_xt))
assert_allclose(s_xt, 11117.79911015)
assert_allclose(d_xt, 11117.79346741)
def test_Ex9_intersect():
# Two paths A and B are given by two pairs of positions:
# Enter elements as lat/long in deg:
n_EA1_E = lat_lon2n_E(rad(10), rad(20))
n_EA2_E = lat_lon2n_E(rad(30), rad(40))
n_EB1_E = lat_lon2n_E(rad(50), rad(60))
n_EB2_E = lat_lon2n_E(rad(70), rad(80))
# Find the intersection between the two paths, n_EC_E:
n_EC_E_tmp = unit(
np.cross(np.cross(n_EA1_E, n_EA2_E, axis=0),
np.cross(n_EB1_E, n_EB2_E, axis=0),
axis=0))
# n_EC_E_tmp is one of two solutions, the other is -n_EC_E_tmp. Select
# the one that is closet to n_EA1_E, by selecting sign from the dot
# product between n_EC_E_tmp and n_EA1_E:
n_EC_E = np.sign(np.dot(n_EC_E_tmp.T, n_EA1_E)) * n_EC_E_tmp
# When displaying the resulting position for humans, it is more
# convenient to see lat, long:
lat_EC, long_EC = n_E2lat_lon(n_EC_E)
msg = 'Ex9, Intersection: lat, long = {} {} deg'
print(msg.format(deg(lat_EC), deg(long_EC)))
assert_allclose(deg(lat_EC), 40.31864307)
assert_allclose(deg(long_EC), 55.90186788)
def change_axes_to_E(n_E, R_Ee=None):
"""
Change axes of the nvector from 'e' to 'E'.
Parameters
----------
n_E: 3 x n array
n-vector [no unit] decomposed in E.
R_Ee : 3 x 3 array
rotation matrix defining the axes of the coordinate frame E.
Returns
-------
n_e: 3 x n array
n-vector [no unit] decomposed in e.
Notes
-----
The function make sure to rotate the coordinates so that axes is 'E':
then x-axis points to the North Pole along the Earth's rotation axis,
and yz-plane coincides with the equatorial plane, i.e.,
y-axis points towards longitude +90deg (east) and latitude = 0.
"""
if R_Ee is None:
R_Ee = E_rotation()
n_E = np.atleast_2d(n_E)
_nvector_check_length(n_E)
n_e = unit(np.matmul(R_Ee, n_E))
return n_e
def mean_horizontal_position(n_EB_E):
"""
Returns the n-vector of the horizontal mean position.
Parameters
----------
n_EB_E: 3 x n array
n-vectors [no unit] of positions Bi, decomposed in E.
Returns
-------
p_EM_E: 3 x 1 array
n-vector [no unit] of the mean positions of all Bi, decomposed in E.
Notes
-----
The result for spherical Earth is returned.
Examples
--------
{super}
"""
n_EM_E = unit(np.sum(n_EB_E, axis=1).reshape((3, 1)))
return n_EM_E
def test_cross_track_distance():
a = [[1.0, 0.0], [0.0, 0.0], [0.0, 1.0]]
b = [[0.0, 0.0], [1.0, 1.0], [0.0, 0.0]]
c = unit([[0.0, 1.0], [1.0, 1.0], [1.0, 0.0]])
path = (a, b)
radius = 6371e3 # mean earth radius [m]
distance = cross_track_distance(path, c, radius=radius)
# print(distance.tolist())
assert_allclose(distance, [-5003771.699005142, 5003771.699005142])
def test_closest_point_on_great_circle():
a = [[1.0, 0.0], [0.0, 0.0], [0.0, 1.0]]
b = [[0.0, 0.0], [1.0, 1.0], [0.0, 0.0]]
c = unit([[0.0, 1.0], [1.0, 1.0], [1.0, 0.0]])
path = (a, b)
radius = 6371e3 # mean earth radius [m]
d = closest_point_on_great_circle(path, c)
print(d.tolist())
assert_allclose(d, [[0.0, 0.0], [1.0, 1.0], [0.0, 0.0]])
on = on_great_circle_path(path, d, radius)
assert on.tolist() == [True, True]
def test_compare_B_frames(self):
E = FrameE(name='WGS84')
E2 = FrameE(name='WGS72')
n_EB_E = E.Nvector(unit([[1], [2], [3]]), z=-400)
B = FrameB(n_EB_E, yaw=10, pitch=20, roll=30, degrees=True)
assert B != E
B2 = FrameB(n_EB_E, yaw=1, pitch=20, roll=30, degrees=True)
assert B != B2
B3 = FrameB(n_EB_E, yaw=10, pitch=20, roll=30, degrees=True)
assert B == B3
n_EC_E = E.Nvector(unit([[1], [2], [2]]), z=-400)
B4 = FrameB(n_EC_E, yaw=10, pitch=20, roll=30, degrees=True)
assert B != B4
n_ED_E = E2.Nvector(unit([[1], [2], [3]]), z=-400)
B5 = FrameB(n_ED_E, yaw=10, pitch=20, roll=30, degrees=True)
assert B != B5
def n_E2R_EN(n_E, R_Ee=None):
"""
Returns the rotation matrix R_EN from n-vector.
Parameters
----------
n_E: 3 x n array
n-vector [no unit] decomposed in E
R_Ee : 3 x 3 array
rotation matrix defining the axes of the coordinate frame E.
Returns
-------
R_EN: 3 x 3 x n array
The resulting rotation matrix [no unit] (direction cosine matrix).
See also
--------
R_EN2n_E, n_E_and_wa2R_EL, R_EL2n_E
"""
if R_Ee is None:
R_Ee = E_rotation()
# n_E = np.atleast_2d(n_E)
# _nvector_check_length(n_E)
# n_E = unit(np.matmul(R_Ee, n_E))
n_e = change_axes_to_E(n_E, R_Ee)
# N coordinate frame (North-East-Down) is defined in Table 2 in Gade (2010)
# Find z-axis of N (Nz):
Nz_e = -n_e # z-axis of N (down) points opposite to n-vector
# Find y-axis of N (East)(remember that N is singular at Poles)
# Equation (9) in Gade (2010):
# Ny points perpendicular to the plane
Ny_e_direction = np.cross([[1], [0], [0]], n_e, axis=0)
# formed by n-vector and Earth's spin axis
on_poles = np.flatnonzero(norm(Ny_e_direction, axis=0) == 0)
Ny_e = unit(Ny_e_direction)
Ny_e[:, on_poles] = array([[0], [1], [0]]) # selected y-axis direction
# Find x-axis of N (North):
Nx_e = np.cross(Ny_e, Nz_e, axis=0) # Final axis found by right hand rule
# Form R_EN from the unit vectors:
# R_EN = dot(R_Ee.T, np.hstack((Nx_e, Ny_e, Nz_e)))
Nxyz_e = np.hstack((Nx_e[:, None, ...], Ny_e[:, None, ...], Nz_e[:, None,
...]))
R_EN = mdot(np.swapaxes(R_Ee, 1, 0), Nxyz_e)
return np.squeeze(R_EN)
def n_EA_E_distance_and_azimuth2n_EB_E(n_EA_E, distance_rad, azimuth, R_Ee=None):
"""
Returns position B from azimuth and distance from position A
Parameters
----------
n_EA_E: 3 x k array
n-vector(s) [no unit] of position A decomposed in E.
distance_rad: m array
great circle distance [rad] from position A to B
azimuth: n array
Angle [rad] the line makes with a meridian, taken clockwise from north.
Returns
-------
n_EB_E: 3 x max(k,m,n) array
n-vector(s) [no unit] of position B decomposed in E.
Notes
-----
The result for spherical Earth is returned.
The shape of the output `n_EB_E` is the broadcasted shapes of `n_EA_E`,
`distance_rad` and `azimuth.
Examples
--------
{super}
See also
--------
n_EA_E_and_n_EB_E2azimuth, great_circle_distance_rad
"""
if R_Ee is None:
R_Ee = E_rotation()
n_EA_E, distance_rad, azimuth = np.atleast_1d(n_EA_E, distance_rad, azimuth)
# Step1: Find unit vectors for north and east:
k_east_E = unit(cross(dot(R_Ee.T, [[1], [0], [0]]), n_EA_E, axis=0))
k_north_E = cross(n_EA_E, k_east_E, axis=0)
# Step2: Find the initial direction vector d_E:
d_E = k_north_E * cos(azimuth) + k_east_E * sin(azimuth)
# Step3: Find n_EB_E:
n_EB_E = n_EA_E * cos(distance_rad) + d_E * sin(distance_rad)
return n_EB_E
def great_circle_normal(n_EA_E, n_EB_E):
"""
Returns the unit normal(s) to the great circle(s)
Parameters
----------
n_EA_E, n_EB_E: 3 x k and 3 x m arrays
n-vector(s) [no unit] of position A and B, decomposed in E.
Returns
-------
normal : 3 x max(k, m) array
Unit normal(s)
Notes
-----
The shape of the output `normal` is the broadcasted shapes of `n_EA_E`and `n_EB_E`.
"""
return unit(cross(n_EA_E, n_EB_E, axis=0), norm_zero_vector=np.nan)
def intersect(path_a, path_b):
"""
Returns the intersection(s) between the great circles of the two paths
Parameters
----------
path_a, path_b: tuples of two n-vectors
defining path A and path B, respectively.
Path A and B has shape 2 x 3 x n and 2 x 3 x m, respectively.
Returns
-------
n_EC_E : array of shape 3 x max(n, m)
n-vector(s) [no unit] of position C decomposed in E.
point(s) of intersection between paths.
Notes
-----
The result for spherical Earth is returned.
The shape of the output `n_EC_E` is the broadcasted shapes of `path_a` and `path_b`.
Examples
--------
{super}
"""
n_EA1_E, n_EA2_E = path_a
n_EB1_E, n_EB2_E = path_b
# Find the intersection between the two paths, n_EC_E:
n_EC_E_tmp = unit(cross(cross(n_EA1_E, n_EA2_E, axis=0),
cross(n_EB1_E, n_EB2_E, axis=0), axis=0),
norm_zero_vector=np.nan)
# n_EC_E_tmp is one of two solutions, the other is -n_EC_E_tmp. Select
# the one that is closet to n_EA1_E, by selecting sign from the dot
# product between n_EC_E_tmp and n_EA1_E:
n_EC_E = np.sign(dot(n_EC_E_tmp.T, n_EA1_E)) * n_EC_E_tmp
if np.any(np.isnan(n_EC_E)):
warnings.warn('Paths are Equal. Intersection point undefined. '
'NaN returned.')
return n_EC_E
def test_Ex7_mean_position():
# Three positions A, B and C are given:
# Enter elements as lat/long in deg:
n_EA_E = lat_lon2n_E(rad(90), rad(0))
n_EB_E = lat_lon2n_E(rad(60), rad(10))
n_EC_E = lat_lon2n_E(rad(50), rad(-20))
# Find the horizontal mean position:
n_EM_E = unit(n_EA_E + n_EB_E + n_EC_E)
truth = [0.3841171702926, -0.046602405485689447, 0.9221074857571395]
# The result is best viewed with a figure that shows the n-vectors
# relative to an Earth-model:
# print('Ex7, See figure')
# plot_earth_figure(n_EA_E,n_EB_E,n_EC_E,n_EM_E)
# print(n_EM_E.ravel().tolist())
assert_allclose(n_EM_E.ravel(), truth)
# Alternatively:
n_EM_E = mean_horizontal_position(np.hstack((n_EA_E, n_EB_E, n_EC_E)))
# print(n_EM_E.ravel())
assert_allclose(n_EM_E.ravel(), truth)
def test_Ex2_B_and_delta_in_frame_B_to_C_in_frame_E():
# delta vector from B to C, decomposed in B is given:
p_BC_B = np.r_[3000, 2000, 100].reshape((-1, 1)) # pylint: disable=too-many-function-args
# Position and orientation of B is given:
n_EB_E = unit([[1], [2], [3]]) # unit to get unit length of vector
z_EB = -400
R_NB = zyx2R(rad(10), rad(20), rad(30))
# the three angles are yaw, pitch, and roll
# A custom reference ellipsoid is given (replacing WGS-84):
a, f = 6378135, 1.0 / 298.26 # (WGS-72)
# Find the position of C.
# SOLUTION:
# Step1: Find R_EN:
R_EN = n_E2R_EN(n_EB_E)
# Step2: Find R_EB, from R_EN and R_NB:
R_EB = np.dot(R_EN, R_NB) # Note: closest frames cancel
# Step3: Decompose the delta vector in E:
p_BC_E = np.dot(R_EB, p_BC_B)
# no transpose of R_EB, since the vector is in B
# Step4: Find the position of C, using the functions that goes from one
# position and a delta, to a new position:
n_EC_E, z_EC = n_EA_E_and_p_AB_E2n_EB_E(n_EB_E, p_BC_E, z_EB, a, f)
# When displaying the resulting position for humans, it is more
# convenient to see lat, long:
lat_EC, long_EC = n_E2lat_lon(n_EC_E)
# Here we also assume that the user wants output height (= - depth):
msg = 'Ex2, Pos C: lat, long = {},{} deg, height = {} m'
print(msg.format(deg(lat_EC), deg(long_EC), -z_EC))
assert_allclose(deg(lat_EC), 53.32637826)
assert_allclose(deg(long_EC), 63.46812344)
assert_allclose(z_EC, -406.00719607)
def test_small_and_large_cross_track_distance():
radius = 6371e3 # m, mean Earth radius
n_EA1_E = lat_lon2n_E(rad(5), rad(10))
n_EA2_E = lat_lon2n_E(rad(10), rad(10))
n_EB0_E = lat_lon2n_E(rad(7), rad(10.1))
path = (n_EA1_E, n_EA2_E)
n_EB1_E = closest_point_on_great_circle(path, n_EB0_E)
for s_xt0 in [
np.pi * radius, np.pi / 3 * radius, 10., 0.1, 1e-3, 1e-4, 1e-5,
1e-8
]:
distance_rad = s_xt0 / radius
n_EB_E = n_EA_E_distance_and_azimuth2n_EB_E(n_EB1_E, distance_rad,
np.pi / 2)
n_EB2_E = closest_point_on_great_circle(path, n_EB_E)
s_xt = great_circle_distance(n_EB1_E, n_EB_E, radius)
c_E = unit(np.cross(n_EA1_E, n_EA2_E, axis=0))
s_xt2 = (np.arccos(np.dot(c_E.T, n_EB_E)) - np.pi / 2) * radius
s_xt3 = cross_track_distance(path,
n_EB_E,
method='greatcircle',
radius=radius)
# pylint: disable=invalid-unary-operand-type
s_xt4 = np.arctan2(
-np.dot(c_E.T, n_EB_E),
np.linalg.norm(np.cross(c_E, n_EB_E, axis=0), axis=0)) * radius
rtol = 10**(-min(9 + np.log10(s_xt0), 15))
if s_xt0 <= np.pi / 3 * radius:
assert_allclose(n_EB2_E, n_EB1_E)
assert_allclose(s_xt2, s_xt0, rtol=rtol)
assert_allclose(s_xt3, s_xt0, rtol=rtol)
assert_allclose(s_xt4, s_xt0, rtol=rtol)
assert_allclose(s_xt, s_xt0, rtol=rtol)
def test_Ex2_B_and_delta_in_frame_B_to_C_in_frame_E():
# delta vector from B to C, decomposed in B is given:
# A custom reference ellipsoid is given (replacing WGS-84):
wgs72 = FrameE(name='WGS72')
# Position and orientation of B is given 400m above E:
n_EB_E = wgs72.Nvector(unit([[1], [2], [3]]), z=-400)
frame_B = FrameB(n_EB_E, yaw=10, pitch=20, roll=30, degrees=True)
p_BC_B = frame_B.Pvector(np.r_[3000, 2000, 100].reshape((-1, 1)))
p_BC_E = p_BC_B.to_ecef_vector()
p_EB_E = n_EB_E.to_ecef_vector()
p_EC_E = p_EB_E + p_BC_E
pointC = p_EC_E.to_geo_point()
lat, lon, z = pointC.latlon_deg
# Here we also assume that the user wants output height (= - depth):
assert_allclose(lat, 53.32637826)
assert_allclose(lon, 63.46812344)
assert_allclose(z, -406.00719607)
def closest_point_on_great_circle(path, n_EB_E):
"""
Returns closest point C on great circle path A to position B.
Parameters
----------
path: tuple of two n-vectors of shape 3 x k and 3 x m
Two n-vectors of positions defining path A, decomposed in E.
n_EB_E: 3 x n array
n-vector(s) of position B to find the closest point to.
Returns
-------
n_EC_E: 3 x max(k, m, n) array
n-vector(s) of closest position C on great circle path A
Notes
-----
The shape of the output `n_EC_E` is the broadcasted shapes of `n_EB_E` and
the n-vectors defining path A.
Examples
--------
{super}
See also
--------
cross_track_distance, great_circle_normal
"""
n_EA1_E, n_EA2_E = path
c_E = great_circle_normal(n_EA1_E, n_EA2_E)
c2 = cross(n_EB_E, c_E, axis=0)
n_EC_E = unit(cross(c_E, c2, axis=0))
return n_EC_E * np.sign(np.sum(n_EC_E * n_EB_E, axis=0, keepdims=True))
def course_over_ground(nvectors, a=6378137, f=1.0 / 298.257223563, R_Ee=None, **options):
"""Returns course over ground in radians from nvector positions
Parameters
----------
nvectors: 3 x n array
Positions of vehicle given as n-vectors [no unit] decomposed in E.
a: real scalar, default WGS-84 ellipsoid.
Semi-major axis of the Earth ellipsoid given in [m].
f: real scalar, default WGS-84 ellipsoid.
Flattening [no unit] of the Earth ellipsoid. If f==0 then spherical
Earth with radius a is used in stead of WGS-84.
R_Ee : 3 x 3 array
rotation matrix defining the axes of the coordinate frame E.
window_length: positive odd integer {0}
The length of the Savitzky-Golay filter window (i.e., the number of coefficients).
Default window_length=0, i.e. no smoothing.
polyorder: int {2}
The order of the polynomial used to fit the samples.
polyorder must be less than window_length.
mode: 'mirror', 'constant', {'nearest'}, 'wrap' or 'interp'.
Determines the type of extension to use for the padded signal to
which the filter is applied. When mode is 'constant', the padding
value is given by cval. When the 'nearest' mode is selected (the default)
the extension contains the nearest input value.
When the 'interp' mode is selected, no extension
is used. Instead, a degree polyorder polynomial is fit to the
last window_length values of the edges, and this polynomial is
used to evaluate the last window_length // 2 output values.
cval: scalar, optional
Value to fill past the edges of the input if mode is 'constant'.
Default is 0.0.
Returns
-------
cog: array of length n
angle in radians clockwise from True North to the direction towards
which the vehicle travels.
Notes
-----
Please be aware that this method requires the vehicle positions to be very smooth!
If they are not you should probably smooth it by a window_length corresponding
to a few seconds or so.
See https://www.navlab.net/Publications/The_Seven_Ways_to_Find_Heading.pdf
for an overview of methods to find accurate headings.
Examples
--------
>>> import matplotlib.pyplot as plt
>>> import nvector as nv
>>> lats = nv.rad(59.381509, 59.387647)
>>> lons = nv.rad(10.496590, 10.494713)
>>> nvec = nv.lat_lon2n_E(lats, lons)
>>> COG_rad = nv.course_over_ground(nvec)
>>> dx, dy = np.sin(COG_rad[0]), np.cos(COG_rad[0])
>>> COG = nv.deg(COG_rad)
>>> p_AB_N = n_EA_E_and_n_EB_E2p_AB_N(nvec[:, :1], nvec[:, 1:]).ravel()
>>> ax = plt.figure().gca()
>>> _ = ax.plot(0, 0, 'bo', label='A')
>>> _ = ax.arrow(0,0, dx*300, dy*300, head_width=20, label='COG')
>>> _ = ax.plot(p_AB_N[1], p_AB_N[0], 'go', label='B')
>>> _ = ax.set_title('COG={} degrees'.format(COG))
>>> _ = ax.set_xlabel('East [m]')
>>> _ = ax.set_ylabel('North [m]')
>>> _ = ax.set_xlim(-500, 200)
>>> _ = ax.set_aspect('equal', adjustable='box')
>>> _ = ax.legend()
>>> plt.show() # doctest: +SKIP
>>> plt.close()
See also
--------
n_EA_E_and_n_EB_E2azimuth
"""
nvectors = np.atleast_2d(nvectors)
if nvectors.shape[1] < 2:
return np.nan
window_length = options.pop('window_length', 0)
if window_length > 0:
window_length = _check_window_length(window_length, nvectors[0])
polyorder = options.pop('polyorder', 2)
mode = options.pop('mode', 'nearest')
if mode not in {'nearest', 'interp'}:
warnings.warn('Using {} is not a recommended mode for filtering headings data!'
' Use "interp" or "nearest" mode instead!'.format(mode))
cval = options.pop('cval', 0.0)
normal = savgol_filter(nvectors, window_length, polyorder, axis=1, mode=mode, cval=cval)
else:
normal = nvectors
n_vecs = np.hstack((normal[:, :1], unit(normal[:, :-1] + normal[:, 1:]), normal[:, -1:]))
return n_EA_E_and_n_EB_E2azimuth(n_vecs[:, :-1], n_vecs[:, 1:], a=a, f=f, R_Ee=R_Ee)
def interp_nvectors(t_i, t, nvectors, kind='linear', window_length=0, polyorder=2, mode='interp',
cval=0.0):
"""
Returns interpolated values from nvector data.
Parameters
----------
t_i: real vector of length m
Vector of interpolation times.
t: real vector of length n
Vector of times.
nvectors: 3 x n array
n-vectors [no unit] decomposed in E.
kind: str or int, optional
Specifies the kind of interpolation as a string
('linear', 'nearest', 'zero', 'slinear', 'quadratic', 'cubic'
where 'zero', 'slinear', 'quadratic' and 'cubic' refer to a spline
interpolation of zeroth, first, second or third order) or as an
integer specifying the order of the spline interpolator to use.
Default is 'linear'.
window_length: positive odd integer
The length of the Savitzky-Golay filter window (i.e., the number of coefficients).
Default window_length=0, i.e. no smoothing.
polyorder: int
The order of the polynomial used to fit the samples.
polyorder must be less than window_length.
mode: 'mirror', 'constant', 'nearest', 'wrap' or 'interp'.
Determines the type of extension to use for the padded signal to
which the filter is applied. When mode is 'constant', the padding
value is given by cval.
When the 'interp' mode is selected (the default), no extension
is used. Instead, a degree polyorder polynomial is fit to the
last window_length values of the edges, and this polynomial is
used to evaluate the last window_length // 2 output values.
cval: scalar, optional
Value to fill past the edges of the input if mode is 'constant'.
Default is 0.0.
Returns
-------
result: 3 x m array
Interpolated n-vector(s) [no unit] decomposed in E.
Notes
-----
The result for spherical Earth is returned.
Examples
--------
>>> import matplotlib.pyplot as plt
>>> import numpy as np
>>> import nvector as nv
>>> lat, lon = nv.rad(np.arange(0, 10)), np.sin(nv.rad(np.linspace(-90, 70, 10)))
>>> t = np.arange(len(lat))
>>> t_i = np.linspace(0, t[-1], 100)
>>> nvectors = nv.lat_lon2n_E(lat, lon)
>>> nvectors_i = nv.interp_nvectors(t_i, t, nvectors, kind='cubic')
>>> lati, loni = nv.deg(*nv.n_E2lat_lon(nvectors_i))
>>> h = plt.plot(nv.deg(lon), nv.deg(lat), 'o', loni, lati, '-')
>>> plt.show() # doctest: +SKIP
>>> plt.close()
Interpolate noisy data
>>> n = 50
>>> lat = nv.rad(np.linspace(0, 9, n));
>>> lon = np.sin(nv.rad(np.linspace(-90, 70, n))) + 0.05* np.random.randn(n)
>>> t = np.arange(len(lat))
>>> t_i = np.linspace(0, t[-1], 100)
>>> nvectors = nv.lat_lon2n_E(lat, lon)
>>> nvectors_i = nv.interp_nvectors(t_i, t, nvectors, 'cubic', 31)
>>> [lati, loni] = nv.n_E2lat_lon(nvectors_i)
>>> h = plt.plot(nv.deg(lon), nv.deg(lat), 'o', nv.deg(loni), nv.deg(lati), '-')
>>> plt.show() # doctest: +SKIP
>>> plt.close()
"""
normal_i = _interp_vectors(t_i, t, nvectors, kind, window_length, polyorder, mode, cval)
return unit(normal_i, norm_zero_vector=np.nan)
def p_EB_E2n_EB_E(p_EB_E, a=6378137, f=1.0 / 298.257223563, R_Ee=None):
"""
Converts Cartesian position vector in meters to n-vector.
Parameters
----------
p_EB_E: 3 x n array
Cartesian position vector(s) [m] from E to B, decomposed in E.
a: real scalar, default WGS-84 ellipsoid.
Semi-major axis of the Earth ellipsoid given in [m].
f: real scalar, default WGS-84 ellipsoid.
Flattening [no unit] of the Earth ellipsoid. If f==0 then spherical
Earth with radius a is used in stead of WGS-84.
R_Ee : 3 x 3 array
rotation matrix defining the axes of the coordinate frame E.
Returns
-------
n_EB_E: 3 x n array
n-vector(s) [no unit] of position B, decomposed in E.
depth: 1 x n array
Depth(s) [m] of system B, relative to the ellipsoid (depth = -height)
Notes
-----
The position of B (typically body) relative to E (typically Earth) is
given into this function as cartesian position vector `p_EB_E`, in meters.
("ECEF-vector"). The function converts to n-vector, `n_EB_E` and its `depth`.
The calculation is excact, taking the ellipsity of the Earth into account.
It is also non-singular as both n-vector and p-vector are non-singular
(except for the center of the Earth).
The default ellipsoid model used is WGS-84, but other ellipsoids/spheres
might be specified.
Examples
--------
{super}
See also
--------
n_EB_E2p_EB_E, n_EA_E_and_p_AB_E2n_EB_E, n_EA_E_and_n_EB_E2p_AB_E
"""
if R_Ee is None:
# R_Ee selects correct E-axes, see E_rotation for details
R_Ee = E_rotation()
# Make sure to rotate the coordinates so that:
# x -> north pole and yz-plane coincides with the equatorial
# plane before using equation 23!
p_EB_e = np.matmul(R_Ee, p_EB_E)
# The following code implements equation (23) from Gade (2010):
x_scale, yz_scale, depth = _equation23(a, f, p_EB_e)
n_EB_e_x = x_scale * p_EB_e[0, :]
n_EB_e_y = yz_scale * p_EB_e[1, :]
n_EB_e_z = yz_scale * p_EB_e[2, :]
n_EB_e = np.vstack((n_EB_e_x, n_EB_e_y, n_EB_e_z))
# Rotate back to the original coordinate system.
n_EB_E = unit(np.matmul(R_Ee.T, n_EB_e)) # Ensure unit length
return n_EB_E, depth
def test_unit(x, y, z):
vec = unit([[x], [y], [z]])
if np.all(np.abs([x, y, z]) < np.inf):
assert_allclose(np.sqrt(np.sum(vec**2)), 1.0)
else:
assert np.all(np.isnan(vec))
