def qmult(p0, pvec, q0, qvec):
"""
Quaternion Multiplications r = p x q
Parameters
----------
p0, q0 : {(N,)} array_like
Scalar componenet of the quaternion
pvec, qvec : {(N,3)} array_like
Vector component of the quaternion
Returns
-------
r0 : {(N,)} array like scalar componenet of the quaternion
rvec : {(N,3)} array like vector component of the quaternion
Examples
--------
>>> import numpy as np
>>> from navpy import qmult
>>> p0, pvec = 0.701057, np.array([-0.69034553, 0.15304592, 0.09229596])
>>> q0, qvec = 0.987228, np.array([ 0.12613659, 0.09199968, 0.03171637])
>>> qmult(q0,qvec,p0,pvec)
(0.76217346258977192, array([-0.58946236, 0.18205109, 0.1961684 ]))
>>> s0, svec = 0.99879, np.array([ 0.02270747, 0.03430854, -0.02691584])
>>> t0, tvec = 0.84285, np.array([ 0.19424161, -0.18023625, -0.46837843])
>>> qmult(s0,svec,t0,tvec)
(0.83099625967941704, array([ 0.19222498, -0.1456937 , -0.50125456]))
>>> qmult([p0, s0],[pvec, svec],[q0, t0], [qvec, tvec])
(array([ 0.76217346, 0.83099626]), array([[-0.59673664, 0.24912539, 0.03053588], [ 0.19222498, -0.1456937 , -0.50125456]]))
"""
p0, Np = _input_check_Nx1(p0)
q0, Nq = _input_check_Nx1(q0)
if (Np != Nq):
raise ValueError('Inputs are not of the same dimension')
pvec, Np = _input_check_Nx3(pvec)
if (Np != Nq):
raise ValueError('Inputs are not of the same dimension')
qvec, Nq = _input_check_Nx3(qvec)
if (Np != Nq):
raise ValueError('Inputs are not of the same dimension')
if (Np > 1):
r0 = p0 * q0 - np.sum(pvec * qvec, axis=1)
else:
r0 = p0 * q0 - np.dot(pvec, qvec)
rvec = p0.reshape(Np, 1) * qvec + q0.reshape(Np, 1) * pvec + np.cross(
pvec, qvec)
# For only 1-D input, make it into a flat 1-D array
if (Np == 1):
rvec = rvec.reshape(3)
return r0, rvec
def ecef2lla(ecef, latlon_unit='deg'):
"""
Calculate the Latitude, Longitude and Altitude of a point located on earth
given the ECEF Coordinates.
References
----------
.. [1] Jekeli, C.,"Inertial Navigation Systems With Geodetic
Applications", Walter de Gruyter, New York, 2001, pp. 24
Parameters
----------
ecef : {(N,3)} array like input of ECEF coordinate in X, Y, and Z column, unit is meters
latlon_unit : {('deg','rad')} specifies the output latitude and longitude unit
Returns
-------
lat : {(N,)} array like latitude in unit specified by latlon_unit
lon : {(N,)} array like longitude in unit specified by latlon_unit
alt : {(N,)} array like altitude in meters
"""
ecef, N = _input_check_Nx3(ecef)
ecef = ecef.reshape(N, 3)
x = ecef[:, 0]
y = ecef[:, 1]
z = ecef[:, 2]
lon = np.arctan2(y, x)
# Iteration to get Latitude and Altitude
p = np.sqrt(x**2 + y**2)
lat = np.arctan2(z, p * (1 - wgs84._ecc_sqrd))
err = np.ones(N)
h = np.zeros(N)
while (np.max(np.abs(err)) > 1e-10):
Rew, Rns = earthrad(lat, lat_unit='rad')
idx = (np.pi / 2 * np.ones(N) - np.abs(lat)) > 1e-3
# For lat < 90 degrees
h[idx] = np.divide(p[idx], np.cos(lat[idx])) - Rew[idx]
# For lat == 90 degrees
h[~idx] = np.divide(z[~idx], np.sin(
lat[~idx])) - (1 - wgs84._ecc_sqrd) * Rew[~idx]
err = np.arctan2(z + wgs84._ecc_sqrd * Rew * np.sin(lat), p) - lat
lat = lat + err
if (latlon_unit == 'deg'):
lat = np.rad2deg(lat)
lon = np.rad2deg(lon)
if (N > 1):
return lat, lon, h
else:
return lat[0], lon[0], h[0]
def quat2dcm(q0, qvec, rotation_sequence='ZYX', output_type='ndarray'):
"""
Convert a single unit quaternion to one DCM
Parameters
----------
q0 : {(N,), (N,1), or (1,N)} array_like
Scalar componenet of the quaternion
qvec : {(N,3),(3,N)} array_like
Vector component of the quaternion
rotation_sequence : {'ZYX'}, optional
Rotation sequences. Default is 'ZYX'.
output_type : {'ndarray','matrix'}, optional
Output is either numpy array (default) or numpy matrix.
Returns
-------
C_N2B : direction consine matrix that rotates the vector from the first frame
to the second frame according to the specified rotation_sequence.
Examples
--------
>>> import numpy as np
>>> from navpy import quat2dcm
>>> q0 = 1
>>> qvec = [0, 0, 0]
>>> C = quat2dcm(q0,qvec)
>>> C
array([[ 1., 0., 0.],
[ 0., 1., 0.],
[ 0., 0., 1.]])
>>> q0 = 0.9811
>>> qvec = np.array([-0.0151, 0.0858, 0.1730])
>>> C = quat2dcm(q0,qvec,output_type='matrix')
>>> C
matrix([[ 9.25570440e-01, 3.36869440e-01, -1.73581360e-01],
[ -3.42051760e-01, 9.39837700e-01, 5.75800000e-05],
[ 1.63132160e-01, 5.93160200e-02, 9.84972420e-01]])
"""
# Input check
q0, N0 = _input_check_Nx1(q0)
qvec, Nvec = _input_check_Nx3(qvec)
if ((N0 != 1) | (Nvec != 1)):
raise ValueError('Can only process 1 quaternion')
q1 = qvec[0]
q2 = qvec[1]
q3 = qvec[2]
if (rotation_sequence == 'ZYX'):
C_N2B = np.zeros((3, 3))
C_N2B[0, 0] = 2 * q0**2 - 1 + 2 * q1**2
C_N2B[1, 1] = 2 * q0**2 - 1 + 2 * q2**2
C_N2B[2, 2] = 2 * q0**2 - 1 + 2 * q3**2
C_N2B[0, 1] = 2 * q1 * q2 + 2 * q0 * q3
C_N2B[0, 2] = 2 * q1 * q3 - 2 * q0 * q2
C_N2B[1, 0] = 2 * q1 * q2 - 2 * q0 * q3
C_N2B[1, 2] = 2 * q2 * q3 + 2 * q0 * q1
C_N2B[2, 0] = 2 * q1 * q3 + 2 * q0 * q2
C_N2B[2, 1] = 2 * q2 * q3 - 2 * q0 * q1
else:
raise ValueError('rotation_sequence unknown')
if (output_type == 'matrix'):
C_N2B = np.asmatrix(C_N2B)
return C_N2B
def quat2angle(q0, qvec, output_unit='rad', rotation_sequence='ZYX'):
"""
Convert a unit quaternion to the equivalent sequence of angles of rotation
about the rotation_sequence axes.
This function can take inputs in either degree or radians, and can also
batch process a series of rotations (e.g., time series of quaternions).
By default this function assumes aerospace rotation sequence but can be
changed using the ``rotation_sequence`` keyword argument.
Parameters
----------
q0 : {(N,), (N,1), or (1,N)} array_like
Scalar componenet of the quaternion
qvec : {(N,3),(3,N)} array_like
Vector component of the quaternion
rotation_sequence : {'ZYX'}, optional
Rotation sequences. Default is 'ZYX'.
Returns
-------
rotAngle1, rotAngle2, rotAngle3 : {(N,), (N,1), or (1,N)} array_like
They are a sequence of angles about successive axes described by
rotation_sequence.
output_unit : {'rad', 'deg'}, optional
Rotation angles. Default is 'rad'.
Notes
-----
Convert rotation angles to unit quaternion that transforms a vector in F1 to
F2 according to
:math:`v_q^{F2} = q^{-1} \otimes v_q^{F1} \otimes q`
where :math:`\otimes` indicates the quaternion multiplcation and :math:`v_q^F`
is a pure quaternion representation of the vector :math:`v_q^F`. The scalar
componenet of :math:`v_q^F` is zero.
For aerospace sequence ('ZYX'): rotAngle1 = psi, rotAngle2 = the,
and rotAngle3 = phi
Examples
--------
>>> import numpy as np
>>> from navpy import quat2angle
>>> q0 = 0.800103145191266
>>> qvec = np.array([0.4619398,0.3314136,-0.1913417])
>>> psi, theta, phi = quat2angle(q0,qvec)
>>> psi
1.0217702360987295e-07
>>> theta
0.7853982192745731
>>> phi
1.0471976051067484
>>> psi, theta, phi = quat2angle(q0,qvec,output_unit='deg')
>>> psi
5.8543122160542875e-06
>>> theta
45.00000320152342
>>> phi
60.000003088824108
>>> q0 = [ 0.96225019, 0.92712639, 0.88162808]
>>> qvec = np.array([[-0.02255757, 0.25783416, 0.08418598],\
[-0.01896854, 0.34362114, 0.14832854],\
[-0.03266701, 0.4271086 , 0.19809857]])
>>> psi, theta, phi = quat2angle(q0,qvec,output_unit='deg')
>>> psi
array([ 9.99999941, 19.99999997, 29.9999993 ])
>>> theta
array([ 30.00000008, 39.99999971, 50.00000025])
>>> phi
array([ -6.06200867e-07, 5.00000036e+00, 1.00000001e+01])
"""
q0, N0 = _input_check_Nx1(q0)
qvec, Nvec = _input_check_Nx3(qvec)
if (N0 != Nvec):
raise ValueError('Inputs are not of same dimensions')
if (N0 == 1):
q1 = qvec[0]
q2 = qvec[1]
q3 = qvec[2]
else:
q1 = qvec[:, 0]
q2 = qvec[:, 1]
q3 = qvec[:, 2]
rotAngle1 = np.zeros(N0)
rotAngle2 = np.zeros(N0)
rotAngle3 = np.zeros(N0)
if (rotation_sequence == 'ZYX'):
m11 = 2 * q0**2 + 2 * q1**2 - 1
m12 = 2 * q1 * q2 + 2 * q0 * q3
m13 = 2 * q1 * q3 - 2 * q0 * q2
m23 = 2 * q2 * q3 + 2 * q0 * q1
m33 = 2 * q0**2 + 2 * q3**2 - 1
rotAngle1 = np.arctan2(m12, m11)
rotAngle2 = np.arcsin(-m13)
rotAngle3 = np.arctan2(m23, m33)
else:
raise ValueError('rotation_sequence unknown')
if (output_unit == 'deg'):
rotAngle1 = np.rad2deg(rotAngle1)
rotAngle2 = np.rad2deg(rotAngle2)
rotAngle3 = np.rad2deg(rotAngle3)
return rotAngle1, rotAngle2, rotAngle3
def ecef2ned(ecef,
lat_ref,
lon_ref,
alt_ref,
latlon_unit='deg',
alt_unit='m',
model='wgs84'):
"""
Transform a vector resolved in ECEF coordinate to its resolution in the NED
coordinate. The center of the NED coordiante is given by lat_ref, lon_ref,
and alt_ref.
Parameters
----------
ecef : {(N,3)} input vector expressed in the ECEF frame
lat_ref : Reference latitude, unit specified by latlon_unit, default in deg
lon_ref : Reference longitude, unit specified by latlon_unit, default in deg
alt : Reference altitude, unit specified by alt_unit, default in m
Returns
-------
ned : {(N,3)} array like ecef position, unit is the same as alt_unit
Examples
--------
>>> import numpy as np
>>> from navpy import ecef2ned
>>> lat
"""
lat_ref, N1 = _input_check_Nx1(lat_ref)
lon_ref, N2 = _input_check_Nx1(lon_ref)
alt_ref, N3 = _input_check_Nx1(alt_ref)
if ((N1 != 1) or (N2 != 1) or (N3 != 1)):
raise ValueError('Reference Location can only be 1')
ecef, N = _input_check_Nx3(ecef)
ecef = ecef.T
C = np.zeros((3, 3))
if (latlon_unit == 'deg'):
lat_ref = np.deg2rad(lat_ref)
lon_ref = np.deg2rad(lon_ref)
elif (latlon_unit == 'rad'):
pass
else:
raise ValueError('Input unit unknown')
C[0, 0] = -np.sin(lat_ref) * np.cos(lon_ref)
C[0, 1] = -np.sin(lat_ref) * np.sin(lon_ref)
C[0, 2] = np.cos(lat_ref)
C[1, 0] = -np.sin(lon_ref)
C[1, 1] = np.cos(lon_ref)
C[1, 2] = 0
C[2, 0] = -np.cos(lat_ref) * np.cos(lon_ref)
C[2, 1] = -np.cos(lat_ref) * np.sin(lon_ref)
C[2, 2] = -np.sin(lat_ref)
ned = np.dot(C, ecef)
ned = ned.T
if (N == 1):
ned = ned.reshape(3)
return ned
def ned2ecef(ned,
lat_ref,
lon_ref,
alt_ref,
latlon_unit='deg',
alt_unit='m',
model='wgs84'):
"""
Transform a vector resolved in NED (origin given by lat_ref, lon_ref, and alt_ref)
coordinates to its ECEF representation.
Parameters
----------
ned : {(N,3)} input array, units of meters
lat_ref : Reference latitude, unit specified by latlon_unit, default in deg
lon_ref : Reference longitude, unit specified by latlon_unit, default in deg
alt_ref : Reference altitude, unit specified by alt_unit, default in m
Returns
-------
ecef : {(N,3)} array like ned vector, in the ECEF frame, units of meters
Notes
-----
The NED vector is treated as a relative vector, and hence the ECEF representation
returned is NOT converted into an absolute coordinate. This means that the
magnitude of `ned` and `ecef` will be the same (bar numerical differences).
Examples
--------
>>> import navpy
>>> ned = [0, 0, 1]
>>> lat_ref, lon_ref, alt_ref = 45.0, -93.0, 250.0 # deg, meters
>>> ecef = navpy.ned2ecef(ned, lat_ref, lon_ref, alt_ref)
>>> print("NED:", ned)
>>> print("ECEF:", ecef)
>>> print("Notice that 'down' is not same as 'ecef-z' coordinate.")
"""
lat_ref, N1 = _input_check_Nx1(lat_ref)
lon_ref, N2 = _input_check_Nx1(lon_ref)
alt_ref, N3 = _input_check_Nx1(alt_ref)
if ((N1 != 1) or (N2 != 1) or (N3 != 1)):
raise ValueError('Reference Location can only be 1')
ned, N = _input_check_Nx3(ned)
ned = ned.T
C = np.zeros((3, 3))
if (latlon_unit == 'deg'):
lat_ref = np.deg2rad(lat_ref)
lon_ref = np.deg2rad(lon_ref)
elif (latlon_unit == 'rad'):
pass
else:
raise ValueError('Input unit unknown')
C[0, 0] = -np.sin(lat_ref) * np.cos(lon_ref)
C[0, 1] = -np.sin(lat_ref) * np.sin(lon_ref)
C[0, 2] = np.cos(lat_ref)
C[1, 0] = -np.sin(lon_ref)
C[1, 1] = np.cos(lon_ref)
C[1, 2] = 0
C[2, 0] = -np.cos(lat_ref) * np.cos(lon_ref)
C[2, 1] = -np.cos(lat_ref) * np.sin(lon_ref)
C[2, 2] = -np.sin(lat_ref)
# C defines transoformation: ned = C * ecef. Hence used transpose.
ecef = np.dot(C.T, ned)
ecef = ecef.T
if (N == 1):
ecef = ecef.reshape(3)
return ecef