def test_rotation_matrix(self):
expected_result = np.array([
[-0.94376147, -0.1102527, 0.3117028],
[0.33062712, -0.31471177, 0.88974278],
[0.0, 0.94276235, 0.33346538],
])
result = statistics.rotation_matrix(lat, lon)
np.testing.assert_almost_equal(expected_result, result)
self.assertEqual(type(expected_result), type(result))
def test_rotation_matrix(self):
lat = 19.4792
lon = 70.6931
expected_result = np.array([
[-0.94376114, -0.11025276, 0.31170376],
[0.33062805, -0.31471096, 0.88974272],
[0.0, 0.94276261, 0.33346463],
])
result = statistics.rotation_matrix(lat, lon)
np.array_equal(expected_result, result)
self.assertEqual(type(expected_result), type(result))
def xyz2enu(lat, lon, x, y, z):
"""Convert a column vector in the Cartesian reference frame to a column
vector in the local reference frame.
:param lat: latitude in decimal degrees
:param lon: longitude in decimal degrees
:param x: in metres
:param y: in metres
:param z: in metres
:return: east, north, up in metres
"""
rot_matrix = rotation_matrix(lat, lon)
xyz = np.array([[x], [y], [z]])
enu = rot_matrix.transpose() @ xyz
east = enu[0, 0]
north = enu[1, 0]
up = enu[2, 0]
return east, north, up
def enu2xyz(lat, lon, east, north, up):
"""Convert a column vector in the local reference frame to a column vector
in the Cartesian reference frame.
:param lat: latitude in decimal degrees
:param lon: longitude in decimal degrees
:param east: in metres
:param north: in metres
:param up: in metres
:return: x, y, z in metres
"""
rot_matrix = rotation_matrix(lat, lon)
enu = np.array([[east], [north], [up]])
xyz = rot_matrix @ enu
x = xyz[0, 0]
y = xyz[1, 0]
z = xyz[2, 0]
return x, y, z
