Moving from referene frame body ($\textbf{F}^b$) to the navigation frame ($\textbf{F}^n$) is done via a rotation matrix. All rotation matrices are special orthogonal groups in three dimensions or SO(3). Thus they have the following properties:
$$ R^{-1}(\theta) = R^T(\theta) \\ det(R(\theta)) = 1 $$All rotations can be broken up into their corresponding Euler angles. By convention, rotating vectors in the body frame to the navigation frame is done by:
$$ R_b^n = R_Z(\psi) R_y(\theta) R_x(\phi) = R_{123}(\phi, \theta,\psi) $$Now we can use this to rotate vectors between reference frames:
$$ p^n = R_b^n p^b = R_{123}(\phi, \theta,\psi) p^b \\ v^n = R_{123}(\phi, \theta,\psi) v^b \\ \omega^n = R_{123}(\phi, \theta,\psi) \omega^b \\ F^n = R_{123}(\phi, \theta,\psi) F^b $$where $n$ refers to a navigation frame and $b$ refers to a body frame.
%load_ext autoreload
%autoreload 2
import numpy as np
from numpy import sin, cos, pi, sqrt
from numpy.testing import assert_allclose
deg2rad = pi/180
rad2deg = 180/pi
from rotations import frames
from rotations import rotations
class Nav:
def __init__(self, orig,):
self.r = self.rot_ecef2ned(*orig[:2])
self.o = self.wgs2ecef(*orig)
def ecef2ned(self, pt):
"""ECEF(x,y,z) to NED(x,y,z)"""
#to_nav()
return self.r.dot(pt-self.o)
def ned2ecef(self, pt):
"""ECEF(x,y,z) to NED(x,y,z)"""
#to_ecef
return self.r.T.dot(pt)+self.o
def wgs2ned(self, lat, lon, alt):
x = self.wgs2ecef(lat,lon,alt)
return self.ecef2ned(x)
def rot_ecef2ned(self, lat, lon, degrees=True):
"""ECEF to NED rotation matrix"""
if degrees:
lat *= deg2rad
lon *= deg2rad
return np.array([
[-sin(lat)*cos(lon), -sin(lat)*sin(lon), cos(lat)],
[-sin(lon), cos(lon), 0],
[-cos(lat)*cos(lon), -cos(lat)*sin(lon), -sin(lat)]
])
def wgs2ecef(self, lat,lon, alt, degrees=True):
"""wgs84(lat,lon,alt) to ECEF(x,y,z)"""
if degrees:
lat *= deg2rad
lon *= deg2rad
a = 6378137.0 # WGS semi-major axis
b = 6356752.314245 # WGS semi-minor axis
e = sqrt(1-(b/a)**2)
d = sqrt(1-e**2*sin(lat)**2)
f = (a/d+alt)*cos(lat)
x = f*cos(lon)
y = f*sin(lon)
z = (a*(1-e*e)/d+alt)*sin(lat)
return np.array([x,y,z])
ned = Nav((45,45,0))
print(ned.r)
print(ned.o)
print(np.linalg.norm(ned.o))
[[-0.5 -0.5 0.70710678] [-0.70710678 0.70710678 0. ] [-0.5 -0.5 -0.70710678]] [3194419.14506062 3194419.14506062 4487348.40886573] 6367489.543863376
p = ned.wgs2ecef(45,45,0)
print(p)
m=ned.ecef2ned(p)
print(m)
[3194419.14506062 3194419.14506062 4487348.40886573] [0. 0. 0.]
pp = ned.ned2ecef(m)
print(pp)
assert_allclose(pp,p)
[3194419.14506062 3194419.14506062 4487348.40886573]
ned.wgs2ned(45.1,45,0)
array([ 1.11132698e+04, -9.09494702e-13, 9.69818833e+00])