def test_R2zxy():
x, y, z = rad((10, 20, 30))
R_AB1 = zyx2R(z, y, x)
R_AB = [[0.813798, -0.44097, 0.378522], [0.469846, 0.882564, 0.018028],
[-0.34202, 0.163176, 0.925417]]
assert_array_almost_equal(R_AB, R_AB1)
z1, y1, x1 = R2zyx(R_AB1)
assert_array_almost_equal((x, y, z), (x1, y1, z1))
def test_R2zxy():
x, y, z = rad((10, 20, 30))
R_AB1 = zyx2R(z, y, x)
R_AB = [[0.813798, -0.44097, 0.378522],
[0.469846, 0.882564, 0.018028],
[-0.34202, 0.163176, 0.925417]]
assert_array_almost_equal(R_AB, R_AB1)
z1, y1, x1 = R2zyx(R_AB1)
assert_array_almost_equal((x, y, z), (x1, y1, z1))
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 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]
