def lla2enu(init_lla,point_lla):
n_EA_E = nv.lat_lon2n_E(init_lla[0], init_lla[1])
n_EB_E = nv.lat_lon2n_E(point_lla[0], point_lla[1])
p_AB_E = nv.n_EA_E_and_n_EB_E2p_AB_E(n_EA_E, n_EB_E, init_lla[2], point_lla[2])
R_EN = nv.n_E2R_EN(n_EA_E)
p_AB_N = np.dot(R_EN.T, p_AB_E).ravel()
p_AB_N[0],p_AB_N[1] = p_AB_N[1],p_AB_N[0]
return p_AB_N
def test_n_E_and_wa2R_EL():
n_E = np.array([[0], [0], [1]])
R_EL = n_E_and_wa2R_EL(n_E, wander_azimuth=np.pi / 2)
R_EL1 = [[0, 1.0, 0], [1.0, 0, 0], [0, 0, -1.0]]
assert_array_almost_equal(R_EL, R_EL1)
R_EN = n_E2R_EN(n_E)
assert_array_almost_equal(R_EN, np.diag([-1, 1, -1]))
n_E1 = R_EL2n_E(R_EN)
n_E2 = R_EN2n_E(R_EN)
assert_array_almost_equal(n_E, n_E1)
assert_array_almost_equal(n_E, n_E2)
def test_n_E_and_wa2R_EL():
n_E = np.array([[0], [0], [1]])
R_EL = n_E_and_wa2R_EL(n_E, wander_azimuth=np.pi/2)
R_EL1 = [[0, 1.0, 0],
[1.0, 0, 0],
[0, 0, -1.0]]
assert_array_almost_equal(R_EL, R_EL1)
R_EN = n_E2R_EN(n_E)
assert_array_almost_equal(R_EN, np.diag([-1, 1, -1]))
n_E1 = R_EL2n_E(R_EN)
n_E2 = R_EN2n_E(R_EN)
assert_array_almost_equal(n_E, n_E1)
assert_array_almost_equal(n_E, n_E2)
def test_Ex1_A_and_B_to_delta_in_frame_N():
# Positions A and B are given in (decimal) degrees and depths:
lat_EA, lon_EA, z_EA = rad(1), rad(2), 3
lat_EB, lon_EB, z_EB = rad(4), rad(5), 6
# Find the exact vector between the two positions, given in meters
# north, east, and down, i.e. find p_AB_N.
# SOLUTION:
# Step1: Convert to n-vectors (rad() converts to radians):
n_EA_E = lat_lon2n_E(lat_EA, lon_EA)
n_EB_E = lat_lon2n_E(lat_EB, lon_EB)
# Step2: Find p_AB_E (delta decomposed in E).
# WGS-84 ellipsoid is default:
p_AB_E = n_EA_E_and_n_EB_E2p_AB_E(n_EA_E, n_EB_E, z_EA, z_EB)
# Step3: Find R_EN for position A:
R_EN = n_E2R_EN(n_EA_E)
# Step4: Find p_AB_N
p_AB_N = np.dot(R_EN.T, p_AB_E)
# (Note the transpose of R_EN: The "closest-rule" says that when
# decomposing, the frame in the subscript of the rotation matrix that
# is closest to the vector, should equal the frame where the vector is
# decomposed. Thus the calculation np.dot(R_NE, p_AB_E) is correct,
# since the vector is decomposed in E, and E is closest to the vector.
# In the example we only had R_EN, and thus we must transpose it:
# R_EN'=R_NE)
# Step5: Also find the direction (azimuth) to B, relative to north:
azimuth = np.arctan2(p_AB_N[1], p_AB_N[0])
# positive angle about down-axis
print('Ex1, delta north, east, down = {0}, {1}, {2}'.format(p_AB_N[0],
p_AB_N[1],
p_AB_N[2]))
print('Ex1, azimuth = {0} deg'.format(deg(azimuth)))
assert_array_almost_equal(p_AB_N[0], 331730.23478089)
assert_array_almost_equal(p_AB_N[1], 332997.87498927)
assert_array_almost_equal(p_AB_N[2], 17404.27136194)
assert_array_almost_equal(deg(azimuth), 45.10926324)
def test_Ex1_A_and_B_to_delta_in_frame_N():
# Positions A and B are given in (decimal) degrees and depths:
lat_EA, lon_EA, z_EA = rad(1), rad(2), 3
lat_EB, lon_EB, z_EB = rad(4), rad(5), 6
# Find the exact vector between the two positions, given in meters
# north, east, and down, i.e. find p_AB_N.
# SOLUTION:
# Step1: Convert to n-vectors (rad() converts to radians):
n_EA_E = lat_lon2n_E(lat_EA, lon_EA)
n_EB_E = lat_lon2n_E(lat_EB, lon_EB)
# Step2: Find p_AB_E (delta decomposed in E).
# WGS-84 ellipsoid is default:
p_AB_E = n_EA_E_and_n_EB_E2p_AB_E(n_EA_E, n_EB_E, z_EA, z_EB)
# Step3: Find R_EN for position A:
R_EN = n_E2R_EN(n_EA_E)
# Step4: Find p_AB_N
p_AB_N = np.dot(R_EN.T, p_AB_E)
# (Note the transpose of R_EN: The "closest-rule" says that when
# decomposing, the frame in the subscript of the rotation matrix that
# is closest to the vector, should equal the frame where the vector is
# decomposed. Thus the calculation np.dot(R_NE, p_AB_E) is correct,
# since the vector is decomposed in E, and E is closest to the vector.
# In the example we only had R_EN, and thus we must transpose it:
# R_EN'=R_NE)
# Step5: Also find the direction (azimuth) to B, relative to north:
azimuth = np.arctan2(p_AB_N[1], p_AB_N[0])
# positive angle about down-axis
print('Ex1, delta north, east, down = {0}, {1}, {2}'.format(
p_AB_N[0], p_AB_N[1], p_AB_N[2]))
print('Ex1, azimuth = {0} deg'.format(deg(azimuth)))
assert_array_almost_equal(p_AB_N[0], 331730.23478089)
assert_array_almost_equal(p_AB_N[1], 332997.87498927)
assert_array_almost_equal(p_AB_N[2], 17404.27136194)
assert_array_almost_equal(deg(azimuth), 45.10926324)
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))
# 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_array_almost_equal(deg(lat_EC), 53.32637826)
assert_array_almost_equal(deg(long_EC), 63.46812344)
assert_array_almost_equal(z_EC, -406.00719607)
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))
# 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_array_almost_equal(deg(lat_EC), 53.32637826)
assert_array_almost_equal(deg(long_EC), 63.46812344)
assert_array_almost_equal(z_EC, -406.00719607)
def setNed(self, current, target):
lat_C, lon_C = rad(current.lat), rad(current.lon)
lat_T, lon_T = rad(target.lat), rad(target.lon)
# create an n-vector for current and target
nvecC = nv.lat_lon2n_E(lat_C, lon_C)
nvecT = nv.lat_lon2n_E(lat_T, lon_T)
# create a p-vec from C to T in the Earth's frame
# the zeros are for the depth (depth = -1 * altitude)
p_CT_E = nv.n_EA_E_and_n_EB_E2p_AB_E(nvecC, nvecT, 0, 0)
# create a rotation matrix
# this rotates points from the NED frame to the Earth's frame
R_EN = nv.n_E2R_EN(nvecC)
# rotate p_CT_E so it lines up with current's NED frame
# we use the transpose so we can go from the Earth's frame to the NED frame
n, e, d = np.dot(R_EN.T, p_CT_E).ravel()
#Scale Nedvalues
if n > e:
scaleFactor = abs(1.00000 / n)
else:
scaleFactor = abs(1.00000 / e)
return Nedvalues(n * scaleFactor, e * scaleFactor, d)
def n_E2R_EN(self):
n_E = self.to_nvector().get_xyz(shape=(3, 1))
R_EN = nv.n_E2R_EN(n_E)
return R_EN
def pvec_b_to_lla(forward, right, down, roll, pitch, yaw, lat, lon, alt):
"""
returns the lat lon and alt corresponding to a p-vec in the UAV's body frame
Parameters
----------
forward: float
The number of meters forward of the UAV
right: float
The number of meters to the right of the UAV
down: float
The number of meters below the UAV
roll: float
The UAV's roll angle in degrees
pitch: float
The UAV's pitch angle in degrees
yaw: float
The UAV's yaw angle in degrees
lat: float
The UAV's latitude in degrees
lon: float
The UAV's longitude in degrees
alt: float
The UAV's altitude in meters
Returns
-------
list
This list holds three floats representing the latitude in degrees, longitude in degrees and altitude in meters (in that order).
"""
# create a p-vector with the forward, right and down values
p_B = np.array([forward, right, down])
# this matrix can transform a pvec in the body frame to a pvec in the NED frame
rot_NB = nv.zyx2R(radians(yaw), radians(pitch), radians(roll))
# calculate the pvec in the NED frame
p_N = rot_NB.dot(p_B)
# create an n-vector for the UAV
n_UAV = nv.lat_lon2n_E(radians(lat), radians(lon))
# this creates a matrix that rotates pvecs from NED to ECEF
rot_EN = nv.n_E2R_EN(n_UAV)
# find the offset vector from the UAV to the point of interest in the ECEF frame
p_delta_E = rot_EN.dot(p_N)
# find the p-vector for the UAV in the ECEF frame
p_EUAV_E = nv.n_EB_E2p_EB_E(n_UAV, -alt).reshape(1, 3)[0]
# find the p-vector for the point of interest. This is the UAV + the offset in the ECEF frame.
p_E = p_EUAV_E + p_delta_E
# find the n-vector for the point of interest given the p-vector in the ECEF frame.
n_result, z_result = nv.p_EB_E2n_EB_E(p_E.reshape(3, 1))
# convert the n-vector to a lat and lon
lat_result, lon_result = nv.n_E2lat_lon(n_result)
lat_result, lon_result = degrees(lat_result), degrees(lon_result)
# convert depth to alt
alt_result = -z_result[0]
return [lat_result, lon_result, alt_result]
