Module ephemeris_library.rotation_libraries.lib_quaternion
Expand source code
DOCUMENTATION_WARNING = """
=== DOCUMENTATION WARNING ===
=============================
All Quaternions provided in this library are described in the 'Right-Hand' notation method.
Such that: vB = qB<-A * vA = qB<-A* . [0,vA] . qB<-A == q*.v.q
Many documentation sources, such as Wikipedia's 'Quaternion' article are 'Left-Hand' notated,
Such that: vB = qB<-A * vA = qB<-A . [0,vA] . qB<-A* == q.v.q*
Note that this just means that the notation methodology just means
the quaternions are conjugates (inversions) of one another.
All rotation chaining is best shown using 'Right-Hand' arrow notation
qA->B . qB->C = qA->C # Right-Arrow notation.
qB<-A . qC<-B = qC<-A # Left-arrow notation.
All processing is done with Scalar-First quaternions. q = [r,i,j,k]
It is highly recommended to avoid the use of this library altogether, except in the
derivation of the GEO/ITRF vectors from the body-frame that the quaternions provide.
It is highly recommended to use the 'quaternions.py' library in the parent folder of the
ephemeris_library.
Note that the right-handedness is -required- for all quaternions.
The Quaternion_to_Rotation_Matrix function does provided Left-haned rotation matricies.
"""
import numpy
from scipy import linalg # For use in the 'Linear_Average' function. Needs Eigenvalues.
################
### Useful Constants:
ZERO_QUAT = numpy.array([0, 0, 0, 0], dtype=float) # The basis 'zero' Quaternion. A Placeholder indicating an invalid rotation.
NULL_QUAT = numpy.array([1, 0, 0, 0], dtype=float) # The basis 'null' Quaternion. A placeholder indicating a non-rotation.
########################
### Default Constructors:
def qZero(Length=0): # Builds array of zero-quaternions.
return numpy.full((Length, 4), ZERO_QUAT, dtype=float)
def qNull(Length=0): # Builds array of null-quaternions.
return numpy.full((Length, 4), NULL_QUAT, dtype=float)
#####################
### Quaternion Masking functions:
def _Is_Zero_Quat(A): # Builds boolean mask if the quaternion-norm is near zero.
return numpy.linalg.norm(A, axis=1) < 1e-10
def _Is_Null_Quat(A): # Builds boolean mask if the vector-component approaches zero.
return numpy.linalg.norm(A[:, 1:], axis=1) < 1e-10
#####################
### Input sanitization:
def _Sanitize_Quaternions( Data ):
'''
Given a data block, make sure it's an NP-array of shape=Nx4, dtype=float
Accepts lists, np-array, etc. Must be float-compatible.
Trusts the user to not be a complete moron on input shaping.
Should be simply castable to an Nx4 array.
Dont' give it staggered arrays, arrays of total entry !%4, etc.
'''
# Force to numpy, cast as float.
Data = numpy.array(Data, dtype=float) # Instantaneous if already nd.array and float.
# Optional upgrade to numpy.longdouble, which doubles the float-resolution (64->128 bits)
Data = Data.reshape( -1, 4 ) # Relies on the user to not be a moron on inputs.
return Data
###############################################
######## Direct Quaternion Mathematics ########
###############################################
def Vector_Rotate( qA_to_B, vA ):
'''
Given quaternions, and a list of vectors in the 'source' frame,
Return the vector in the 'destination' frame.
Rotation for this is q*.v.q, using Right-Hand quaternions.
[0,vB] = qB<-A* . [0,vA] . qB<-A
= qA<-B . [0,vA] . qB<-A # Left-arrow notation.
= qB->A . [0,vA] . qA->B # Right-arrow notation.
Equivalent to the left-Handed rotation-matrix rotation of
vB = Rot(A->B) . vA
Args:
qA_to_B: Quaternion list of floats expressing the rotation from frame 'A' into frame 'B'.
Expressed as 'qA->B' or 'qB<-A'
vA: A single or list of 3-vectors of floats that expresses a vector in frame 'A'.
Returns:
vB: A list of 3-vectors of floats that expresses vectors in frame 'B'.
'''
qA_to_B = _Sanitize_Quaternions( qA_to_B )
q = qA_to_B
qC = qB_to_A = Conjugate(qA_to_B) # And it's conjugate.
# And do a coarse sanitization of the vectors:
vA = numpy.array(vA, dtype=float).reshape(-1,3)
# Prepend a zero to the Vector to turn it into a zero-scalar quaternion.
qVec_A = numpy.column_stack((numpy.zeros(len( vA )), vA )) # [0, v]
qVec_A = _Sanitize_Quaternions( qVec_A ) # And then sanitize to check input shaping.
# If either is empty, return an empty quaternion array.
if len( qA_to_B ) == 0 or len(vA) == 0:
return qZero(0) # Returns Zero-Vectors, empty.
# Apply the rotation
qVec_B = _qChain_Multiply(( qC, qVec_A, q ))
# And then remove the zero-scalar value.
vB = qVec_B[:, 1:]
return vB
def Geodesic_Norm(A, B):
'''
Given two quaternions, return the total angle in degrees between them.
Must be of equal shape, describing the same rotation.
Can provide a Null Quaternion [1,0,0,0] as B to get the total rotation of A
'''
A = Normalize( _Sanitize_Quaternions(A) )
B = Normalize( _Sanitize_Quaternions(B) )
if A.shape != B.shape:
raise ValueError("Geodesic Norm given sets of unequal shape!")
Dot = numpy.sum(A * B, axis=1)
Dot[Dot < -1.0] = -1.0
Dot[Dot > 1.0] = 1.0
return numpy.arccos(2 * Dot**2 - 1) * 180 / numpy.pi
def _qMultiply( qA_to_B, qB_to_C ): # qA->B , qB->C
'''
Given two quaternions, qA->B, qB->C, return qA->C, (==qC<-A)
Rotation for this is q1.q2 = q3, where the dot is the Hamilton Product.
Requires Right-Handed quaternions. Best described using Right-Arrow notation:
qA->B . qB->C = qA->C
Equivalent to the Rotation Matrix of: R(C<-B) . R(B<-A), where the dot is the dot product of the two matricies.
'''
qA_to_B = _Sanitize_Quaternions( qA_to_B )
qB_to_C = _Sanitize_Quaternions( qB_to_C )
# If either empty, return empty.
if len( qA_to_B ) == 0 or len( qB_to_C ) == 0: return qZero(0)
# And if both aren't of the same dimentions (when either not single-quats), then raise error.
if len( qA_to_B ) not in [1, len(qB_to_C)] and len(qB_to_C) != 1:
raise ValueError(
"Given Quaternions lists with mismatched dimentions! A:{:d}x{:d} B:{:d}x{:d}".format(*qA_to_B.shape, *qB_to_C.shape))
# Following math is the q2.q1=q3 math:
# Split the quaternions into their columns.
a, b, c, d = qA_to_B.T # qA->B
e, f, g, h = qB_to_C.T # qB->C
qR = a * e - b * f - c * g - d * h # Scalar
qI = a * f + b * e + c * h - d * g # *I
qJ = a * g - b * h + c * e + d * f # *J
qK = a * h + b * g - c * f + d * e # *K
qA_to_C = numpy.column_stack((qR, qI, qJ, qK)) # == qA->C == qC<-A
return qA_to_C
def _qChain_Multiply( List_qSource_to_Dest ): # [ qA->B ]*
'''
Given a list of Right-Handed quaternions, of chaining quaternions using Right-arrow notation:
Eg: qA->B, qB->C, qC->D, qD->E
Return a quaternion from the first 'Source' to the final 'Destination' frame
Eg: Above returns qA->E == qE<-A
Simply provides a cleaner interface than a hard-to-read "Mul( Mul( Mul(A,B) ,C) ,D)"
'''
# If None, or any empty, return empty.
if List_qSource_to_Dest is None or any([len(q) == 0 for q in List_qSource_to_Dest]):
return qZero(0)
# Sanitize all the inputs:
List_qSource_to_Dest = [ _Sanitize_Quaternions( q ) for q in List_qSource_to_Dest ]
# Build a temp variable:
qTemp = qNull(1) # Returns an array of [1,0,0,0], a quaternion describing a non-rotation.
# This expands to the length of whatever is neeed, just acting as an x1 multiplier.
# And then chain together the rotations:
for qPrev_to_Next in List_qSource_to_Dest:
qTemp = _qMultiply( qTemp, qPrev_to_Next )
qFirst_To_Final = qTemp
return qFirst_To_Final
def _qPower( q, h ):
'''
Given a quaternion and a power, return q^h.
Multiplies the given rotation. Eg: q^2 == q.q
Or fractionalizes the rotation. Eg: q^0.5 == Half the rotation along the same axis.
Valid for full range of h.
Maths:
q^a = ||q||^a * [cos(a*Phi) + N*sin(a*Phi)]
Phi = acos( qR / ||q||) # The Half-Angle of rotation.
N = v / ||v|| # The normalized quaternion-component vector
'''
q = _Sanitize_Quaternions(q) # Forces into a clean Nx4 array of quats.
h = numpy.array(h, dtype=float).reshape(-1) # Forces into a linear-array of floats.
Len_Q = numpy.linalg.norm(q, axis=1) # Get the lengths of each Quaternion in the list
Len_V = numpy.linalg.norm(q[:, 1:], axis=1) # Get the lengths of every Quaternion, discarding the Scalar component.
# Sanity. In case fed [0,0,0,0] Quaternions. Make sure to override at the end to continue to let them be null quats.
Zero_Quats = Len_Q == 0 # Bool list of given [0,0,0,0] Quaternions.
Len_Q[Zero_Quats] = 1 # As to avoid divide-by-zero errors.
qR = q[:, 0] # Extract only the Scalar Value
qV = q[:, 1:] # Extract only the Vector component
Phi = numpy.arccos(qR / Len_Q)
N = Normalize(qV)
# For excedingly small values of len_V, approaching a Null Quaternion (or [0,0,0,0], set to zero.
N[Len_V < 1e-10] = 0
qR = Len_Q**h * numpy.cos(h * Phi)
qV = (Len_Q**h * numpy.sin(h * Phi) * N.T).T
# Assemble the final array.
Q_Output = numpy.column_stack((qR, qV))
# And override all the given zero-quats as zeros again.
Q_Output[Zero_Quats] = numpy.zeros((Q_Output.shape))[Zero_Quats]
return Q_Output
def _qExponent( q ):
'''
Given a quaternion, returns e^q
Uses Euler's Formula, treating each imaginary value as it's own component.
e^xi = cos(x) + i.sin(x)
e^q = e^(A+Bi+Cj+Dk)
= e^A . e^Bi . e^Cj . e^Dk
And then each is calculated independently.
Maths:
e^q = q^(R) * [cos(|v|) , v/|v| * sin(|v|)]
e^Null = Null
e^ln(Null) = Null
'''
q = Normalize(_Sanitize_Quaternions( q )) # Sanitize and Normalize the Quaternions
is_Null_Quat = _Is_Null_Quat( q ) # Boolean array of if the vector-component approaches zero.
is_Zero_Quat = _Is_Zero_Quat( q ) # Boolean array of if the entire quaternion is near zero.
qR = q[:, 0 ] # The scalar-component.
qV = q[:, 1:] # The vector-component.
Len_V = numpy.linalg.norm(qV, axis=1)
Len_V[Len_V == 0] = 1e-15 # Override zero-vectors as to avoid divide-by-zero.
eScalar = numpy.cos(Len_V) * numpy.e ** qR
eVector = (numpy.sin(Len_V) * numpy.e ** qR / Len_V * qV.T).T
eQuat = numpy.column_stack((eScalar, eVector))
# And for Quaternions with floating-point-error sized Len_V, set as the null quaternion.
eQuat[is_Null_Quat] = NULL_QUAT
eQuat[is_Zero_Quat] = ZERO_QUAT # And do the same with the false-quats.
eQuat[Len_V > 1 - 1e15] = NULL_QUAT # For e^ln(Null) = Null.
return eQuat
def _qLog( q ):
'''
Given quaternions, the natural logarithm of the quaternion.
As any quaternion can be expressed as a value of ||q||.e^( n.o )
n = vector: A unit vector of the rotation vector
o = omega : An angle of the rotation amount.
The logarithm is simply an extraction of this.
Maths:
ln(Q) = ln(|q|) , v/|v| * acos(qR / |q|)
ln(Null) = [0,1,0,0]
'''
q = Normalize(_Sanitize_Quaternions( q ))
is_Null_Quat = _Is_Null_Quat( q ) # Boolean array of if the vector-component approaches zero.
is_Zero_Quat = _Is_Zero_Quat( q ) # Boolean array of if the entire quaternion is near zero.
qR = q[:, 0 ] # The scalar-component.
qV = q[:, 1:] # The vector-component.
# Negate the entire quaternion if the scalar component is negative, as a positive value is needed for the arccos.
_Negate_Mask = qR < 0
qR[_Negate_Mask] *= -1
qV[_Negate_Mask] *= -1
Len_Q = numpy.linalg.norm(q, axis=1)
Len_V = numpy.linalg.norm(qV, axis=1)
Len_Q[Len_Q < 1e-10] = 1e-15 # Divide-by-zero warning suppression. Overriden by the 'is_Null_Quats' below.
Len_V[Len_V < 1e-10] = 1e-15
lnScalar = numpy.log(Len_Q)
lnVector = (numpy.arccos(qR / Len_Q) / Len_V * qV.T).T
lnQuat = numpy.column_stack((lnScalar, lnVector))
# If the vector component is significantly smaller than the real component, then set as [0,1,0,0]
lnQuat[is_Null_Quat] = numpy.array([0, 1, 0, 0]).astype(float)
lnQuat[is_Zero_Quat] = ZERO_QUAT
return lnQuat
#########################################################
################ Quaternion Manipulation ################
def Conjugate(q):
'''
Inverts the quaternion, s.t. it now describes the opposite rotation.
Does so by inverting the vector component, as negating the scalar has edge cases in it's use.
'''
return q * [1, -1, -1, -1]
def Normalize(q):
'''
Given an array of floats, normalize them to a length of 1.0
Valid for Quaternions and Vectors.
Args:
q: NxM np-array of floats.
Returns:
qN: NxM np-array of floats, where each row has a norm/length of 1.0
'''
if len(q) == 0: # If empty, return empty.
return q
q_Norms = numpy.linalg.norm(q, axis=1) # Computes vector lengths.
q_Norms[q_Norms <= 1e-10] = 1 # If given a 'Zero' Quaternion, ~[0,0,0,0], just divide by 1 to avoid Divide-by-zero errors.
q_Normalized = (q.T / q_Norms).T # Quirk of strucure & broadcasting rules, but does the division correctly.
return q_Normalized
#################################################################
#################### Quaternion Translations ####################
def Rotation_Matrix_to_Quaternion(R):
'''
Given arrays of rotation matricies, return their equivalent quaternion.
####
Rotation matricies must be LEFT-HANDED - vB = rB<-A . vA
Returned quaternions are RIGHT-HANDED - vB = qB<-A*. vA . qB<-A = q*.v.q
####
Takes arrays of either Nx3x3 or Nx1x9.
3x3 matrix == 1x9 matrix of the following format:
[A B C]
[D E F] == [A B C D E F G H I]
[G H I]
###
Example Calculation:
qR = 0.5 * (1+ A+E+I)**0.5
qI = (H-F) / 4*qR
qJ = (C-G) / 4*qR
qK = (D-B) / 4*qR
But the divisor entry (qR here) needs to be the largest absolute entry as to avoid division error.
Can calculate qI, qJ, or qJ to also be the divisor.
'''
R = numpy.array(R) # Instant if already in nd.array
if R.shape[1:] not in [(3, 3), (9,)]:
raise ValueError("Must be fed either an Nx3x3 or an Nx9 array")
# If linear array, reshape to 3x3.
if R.shape[1:] == (9,):
R = R.reshape(len(R), 3, 3)
# Rename the solution entries.
A = R[:, 0, 0] # [A B C]
B = R[:, 0, 1] # R->[D E F]
C = R[:, 0, 2] # [G H I]
D = R[:, 1, 0]
E = R[:, 1, 1]
F = R[:, 1, 2]
G = R[:, 2, 0]
H = R[:, 2, 1]
I = R[:, 2, 2]
# Calculate the basis entires. Guaranteed positive.
# AEI == Trace, 3x3 rot-matrix trace range {-1->3}.
# Sanity check against the other variants inside the sqrt all prove positive.
# Though -CAN- have negative with floating point error. The numpy.abs is just for sanity, as the negative value will never be used as the basis.
qR_Basis = 0.5 * numpy.abs(1 + A + E + I)**0.5
qI_Basis = 0.5 * numpy.abs(1 + A - E - I)**0.5
qJ_Basis = 0.5 * numpy.abs(1 - A + E - I)**0.5
qK_Basis = 0.5 * numpy.abs(1 - A - E + I)**0.5
# Build, then extract the best 'basis' entries to be used for the division of each solution.
Basis_Array = numpy.column_stack((qR_Basis, qI_Basis, qJ_Basis, qK_Basis))
# Builds a mask where the row's max index is this index.
qR_Basis_Mask = numpy.max(Basis_Array, axis=1) == Basis_Array[:, 0]
qI_Basis_Mask = numpy.max(Basis_Array, axis=1) == Basis_Array[:, 1] # Use this as the divisor, to minimize error.
qJ_Basis_Mask = numpy.max(Basis_Array, axis=1) == Basis_Array[:, 2] # In the event of A == B, just overwrites.
# No need to test sign, as Basis Array guaranteed positive.
qK_Basis_Mask = numpy.max(Basis_Array, axis=1) == Basis_Array[:, 3]
# Build the relevant rotation-matrix entries.
qRqI = qIqR = (F - H) / 4
qRqJ = qJqR = (G - C) / 4
qRqK = qKqR = (B - D) / 4
qJqK = qKqJ = (F + H) / 4
qIqK = qKqI = (G + C) / 4
qIqJ = qJqI = (B + D) / 4
# Pre-initialize the array.
Q = numpy.zeros((len(A), 4))
# For each entry where the Basis is the greatest, calculate the other components.
if len(qR_Basis_Mask) > 0:
Q[qR_Basis_Mask, 0] = qR_Basis[qR_Basis_Mask]
Q[qR_Basis_Mask, 1] = qIqR[qR_Basis_Mask] / qR_Basis[qR_Basis_Mask]
Q[qR_Basis_Mask, 2] = qJqR[qR_Basis_Mask] / qR_Basis[qR_Basis_Mask]
Q[qR_Basis_Mask, 3] = qKqR[qR_Basis_Mask] / qR_Basis[qR_Basis_Mask]
if len(qI_Basis_Mask) > 0:
Q[qI_Basis_Mask, 0] = qRqI[qI_Basis_Mask] / qI_Basis[qI_Basis_Mask]
Q[qI_Basis_Mask, 1] = qI_Basis[qI_Basis_Mask]
Q[qI_Basis_Mask, 2] = qJqI[qI_Basis_Mask] / qI_Basis[qI_Basis_Mask]
Q[qI_Basis_Mask, 3] = qKqI[qI_Basis_Mask] / qI_Basis[qI_Basis_Mask]
if len(qJ_Basis_Mask) > 0:
Q[qJ_Basis_Mask, 0] = qRqJ[qJ_Basis_Mask] / qJ_Basis[qJ_Basis_Mask]
Q[qJ_Basis_Mask, 1] = qIqJ[qJ_Basis_Mask] / qJ_Basis[qJ_Basis_Mask]
Q[qJ_Basis_Mask, 2] = qJ_Basis[qJ_Basis_Mask]
Q[qJ_Basis_Mask, 3] = qKqJ[qJ_Basis_Mask] / qJ_Basis[qJ_Basis_Mask]
if len(qK_Basis_Mask) > 0:
Q[qK_Basis_Mask, 0] = qRqK[qK_Basis_Mask] / qK_Basis[qK_Basis_Mask]
Q[qK_Basis_Mask, 1] = qIqK[qK_Basis_Mask] / qK_Basis[qK_Basis_Mask]
Q[qK_Basis_Mask, 2] = qJqK[qK_Basis_Mask] / qK_Basis[qK_Basis_Mask]
Q[qK_Basis_Mask, 3] = qK_Basis[qK_Basis_Mask]
return Normalize(Q) # Final sanity check.
def Quaternion_to_Rotation_Matrix(q):
'''
Given a list of quaternions, return their equivalent rotation matricies.
####
Given quaternions must be RIGHT-HANDED - vB = qB<-A*. vA . qB<-A = q*.v.q
Returned rotation matricies are LEFT-HANDED - vB = rB<-A . vA
'''
# Sanity-checking.
q = _Sanitize_Quaternions(q)
q = Normalize(q)
qR, qI, qJ, qK = [q[:, i] for i in range(4)] # Splits the array (either single or bulk) into the individual columns.
A = 1 - 2 * (qJ**2 + qK**2)
B = 2 * (qI * qJ + qK * qR)
C = 2 * (qI * qK - qJ * qR)
D = 2 * (qI * qJ - qK * qR) #[[A,B,C],
E = 1 - 2 * (qI**2 + qK**2) # [D,E,F],
F = 2 * (qJ * qK + qI * qR) # [G,H,I]]
G = 2 * (qI * qK + qJ * qR)
H = 2 * (qJ * qK - qI * qR)
I = 1 - 2 * (qI**2 + qJ**2)
return numpy.dstack(( A,B,C, D,E,F, G,H,I )).reshape(-1, 3, 3)
def YPR_to_Quaternion( YPR ): # NEEDS TESTING
'''
Given an Nx3 list of Yaw, Pitch, and Roll, return a rotation describing that rotation as a quaternion.
If given the YPR as described by the CAS_ephemeris file, return Nadir->CRF/Body quaternions.
The Inverse of the .Quaternion_to_YPR() function.
'''
# NOTE: Uses the ZYX/321 rotation structure, not the 123/XYZ
Yaw, Pitch, Roll = -(YPR * numpy.pi / 180).T # Extract the dataset, in radians
# The above values are negated, as the below derives the 123/XYZ rotation.
# This is then inverted at the final step as to generate the 321/ZYX rotation.
# Shortcuts: the sin/cos of half the angles.
cY, sY = numpy.cos(Yaw / 2), numpy.sin(Yaw / 2)
cP, sP = numpy.cos(Pitch / 2), numpy.sin(Pitch / 2)
cR, sR = numpy.cos(Roll / 2), numpy.sin(Roll / 2)
qR = cR * cP * cY + sR * sP * sY
qI = sR * cP * cY - cR * sP * sY
qJ = cR * sP * cY + sR * cP * sY
qK = cR * cP * sY - sR * sP * cY
Q = numpy.column_stack((qR, qI, qJ, qK))
return Conjugate(Q)
def Quaternion_to_YPR( qNadir_to_CRF ):
'''
Given a list of quaternions,
Return an Nx3 array of Yaw, Pitch, and Roll, in degrees.
To match the CAS_Ephemeris YPR data, must be given Nadir->CRF/Body quaternions.
'''
qNadir_to_CRF = Normalize(_Sanitize_Quaternions(qNadir_to_CRF))
# Note that below derivation uses the 123/XYZ rotation-derivation.
# Derivation of the 321/ZYX is done by inverting the input quaternion (Body->Nadir), and negating the YPR values.
q = Conjugate(qNadir_to_CRF)
qR, qI, qJ, qK = q.T # Splie the Quatenions Column-wise
Roll = -numpy.arctan2(2 * (qR * qI + qJ * qK), 1 - 2 * (qI**2 + qJ**2))
Pitch = -numpy.arcsin (2 * (qR * qJ - qK * qI))
Yaw = -numpy.arctan2(2 * (qR * qK + qI * qJ), 1 - 2 * (qJ**2 + qK**2))
# Note that in the event of a Pitch=+-90*, within a few arcseconds, the Pitch value will return NaN, as this is a Gimble Lock.
# As a workaround, if any row has a NaN, then replace all row with 999/999/999
YPR = numpy.column_stack((Yaw, Pitch, Roll)) * 180 / numpy.pi
YPR[numpy.any(numpy.isnan(YPR), axis=1)] = [999, 999, 999]
return YPR
################################################
########### Quaternion Interpolation ###########
################################################
def _Prep_For_Interpolation( q ):
'''
Given a list of Quaternions, return the normalized, aligned quaternions.
This is needed to be called on a dataset as otherwise the SLERP interpolation will take less-optimal routes.
'''
# If the ~Angle from the prior is greater than 90*, negate it s.t. they are within 90*.
# This does not change the quaternion.
q = _Sanitize_Quaternions( q )
q = Normalize( q )
for i in range(1, len( q )):
if numpy.dot( q[i - 1], q[i] ) < 0:
q[i] *= -1
return q
def _SLERP(qA, qB, h):
'''
SLERP interpolation. Given two lists of quaternions and the desired fraction between them,
Return the spherical-linear interpolated point between each given pair.
Math: qA(qA.C * qB)^h
- h=0/1 will return qA, qB.
- h=0.5 will return the midpoint.
'''
# As this is a lower-level function, throw expections when given out-of-bounds inputs
if max(h) > 1.0 or min(h) < 0.0:
raise ValueError("_SLERP given out of bounds fraction inputs!")
if len(qA) != len(qB) != len(h):
raise ValueError("_SLERP given mismatched input counts!")
qA_Conjugate = Conjugate(qA)
Internal_Brackets = _qMultiply(qA_Conjugate, qB)
Powered_Function = _qPower(Internal_Brackets, h)
Output_Quaternions = _qMultiply(qA, Powered_Function)
# One-liner: Mul(qA, Power(Mul(Conj(qA), qB), h))
# == qA . ( qA* . qB )^h
# And then override any values to prevent floating-point error.
Output_Quaternions[h == 0.0] = qA[h == 0.0]
Output_Quaternions[h == 1.0] = qB[h == 1.0]
return Output_Quaternions
def _SQUAD(qA, qB, aA, aB, h):
'''
Bulk SQUAD interpolation. Given two lists of quaternions, their related Anchor Points, and the fraction between them,
Return the list of quaternions along the curve.
SQUAD interpolation SLERP's two points on the qA/qB line, and the aA/aB line on time-fraction h,
then SLERP'S these two points using a time-fraction 2h(h-1)
== S( S(qA,qB,h), S(aA,aB,h), 2h(1-h))
'''
if len(qA) != len(qB) != len(aA) != len(aB) != len(h):
raise ValueError("_SQUAD given mismatched length attributes")
if len(qA) == 0:
return numpy.zeros((0, 4))
if min(h) < 0.0 or max(h) > 1.0:
raise ValueError("_SQUAD given h outside of {0:1} range!")
return _SLERP(_SLERP(qA, qB, h), _SLERP(aA, aB, h), 2 * h * (1 - h))
def _SLERP_Interpolation_Setup(t_Given, q_Given, t_Saught):
'''
Sets up and runs a SLERP interpolation.
All given values MUST be fully validated.
External calls should use 'Quaternion_Interpolation()' to call this function safely.
Should only be called on datasets with at most two solutions, as otherwise, SQUAD is a more accurate & smoother attitude interpolation.
'''
q_Given = _Prep_For_Interpolation(q_Given) # Make sure the rotations are properly aligned.
q_Saught = qZero(len(t_Saught)) # Pre-Build the quaternion-array to return.
# This should only be called by the parent function if only given two input solutions, so this can be done manually.
t_Low, t_High = t_Given
t_Frac = (t_Saught - t_Low) / (t_High - t_Low)
Valid = numpy.logical_and(0.0 <= t_Frac, t_Frac <= 1.0) # A valid mask of times that are not extrapolated.
t_Frac = t_Frac[Valid] # Mask away the invalid solutions
q_Low = numpy.full((len(t_Frac), 4), q_Given[0]) # Build dummy Nx4 arrays repeating the High & Low input quaternions.
q_High = numpy.full((len(t_Frac), 4), q_Given[1])
q_Saught[Valid] = _SLERP(q_Low, q_High, t_Frac)
return q_Saught
def _SQUAD_Interpolation_Setup(t_Given, q_Given, t_Saught):
'''
Sets up and runs the SQUAD interpolation.
All given values MUST be fully validated.
Use the 'Quaternion_Interpolation' function to call this function safely.
'''
# Make sure the rotations are properly aligned.
q_Given = _Prep_For_Interpolation(q_Given)
# Pre-Build the return array.
q_Saught = qZero(len(t_Saught)) # Builds a Nx4 array of Zero_Quaternions
###########################
###### Build Spline #######
# First, build the anchor points from the given Quaternions
# Anchor[i] = q[i] * e^( ( ln(q*[i].q[i+1]) + ln(q*[i].q[i-1]) ) / -4 )
# The first and last points are just anchored to the given, but the rest need calculation.
Prior_Quats = q_Given[:-2]
Center_Quats = q_Given[1:-1]
Next_Quats = q_Given[2:]
Center_Conj = Conjugate(Center_Quats)
Anchor_Quats = _qLog(_qMultiply(Center_Conj, Next_Quats)) + \
_qLog(_qMultiply(Center_Conj, Prior_Quats)) # = ln(q*[i].q[i+1]) + ln(q*[i].q[i-1])
Anchor_Quats /= -4
Anchor_Quats = _qExponent(Anchor_Quats) # e^(Anchor_Quats)
Anchor_Quats = _qMultiply(Center_Quats, Anchor_Quats) # q[i] * Anchor_Quats
# Then add the first and last good quaternions as the end-point anchors
Anchor_Quats = numpy.concatenate(([q_Given[0]], Anchor_Quats, [q_Given[-1]]))
#################################
###### Build Seek Points ########
# Returns the index of the High-Value if equal. Eg: [1,2,3] [0,1,2,3,4] == [0,1,2,3,3]
Insert_Indecies_Left = numpy.searchsorted(t_Given, t_Saught, 'left')
# Returns the index of the Low-Value if equal. Eg: [1,2,3] [0,1,2,3,4] == [0,0,1,2,3]
Insert_Indecies_Right = numpy.searchsorted(t_Given, t_Saught, 'right')
# Now, as a quirk of these, we need to filter the entries where the insert point is zero OR len(t_Given), and the Left-Insert == Right_Insert.
# This will filter any values that are outside the t_Given range, but keeps the values that are on the edges.
_Outside_Of_Range = ~numpy.logical_and(t_Given[0] <= t_Saught, t_Saught <= t_Given[-1])
_Same_Insert_Point = Insert_Indecies_Left == Insert_Indecies_Right
Valid_t_Saught_Mask = ~numpy.logical_and(_Outside_Of_Range, _Same_Insert_Point)
# Then, for each index that has the same insertion point, is between two time-values in t_Given.
Derived_t_Saught_Mask = numpy.logical_and(Valid_t_Saught_Mask, _Same_Insert_Point)
# And where they are NOT equal, it means they are anchored to a given point.
Anchored_t_Saught_Mask = numpy.logical_and(Valid_t_Saught_Mask, ~_Same_Insert_Point)
###########################
# First, fetch the Anchored_t_Saught points.
# The LHS_Insertion points are equal to their index. Eg: [1,2,3] [2] == [1], so [1,2,3][1] == 2
q_Saught[Anchored_t_Saught_Mask] = q_Given[Insert_Indecies_Left[Anchored_t_Saught_Mask]]
##########################
# Then, fetch the Derived_t_Saught points.
q_Low = q_Given[Insert_Indecies_Left[Derived_t_Saught_Mask] - 1] # The left-side of the input quaternions
q_High = q_Given[Insert_Indecies_Left[Derived_t_Saught_Mask]]
a_Low = Anchor_Quats[Insert_Indecies_Left[Derived_t_Saught_Mask] - 1] # The left-side of the derived 'anchor' points
a_High = Anchor_Quats[Insert_Indecies_Left[Derived_t_Saught_Mask]]
t_Low = t_Given[Insert_Indecies_Left[Derived_t_Saught_Mask] - 1] # The timestamp corresponding to the 'low' side.
t_High = t_Given[Insert_Indecies_Left[Derived_t_Saught_Mask]]
t_Saught_Derived = t_Saught[Derived_t_Saught_Mask]
# The fraction of the time between the 'low' and 'high' sides (prior and following solution)
t_Frac = (t_Saught_Derived - t_Low) / (t_High - t_Low)
# Feed these all into the SQUAD interpolator.
q_Saught[Derived_t_Saught_Mask] = _SQUAD(q_Low, q_High, a_Low, a_High, t_Frac)
return q_Saught
# Given a list of non-zero quaternions and their related timestamps,
# If given 0 solutions, return a Zero-quaternion (Invalid) for each t_Saught.
# If given 1 solution, and any mismatching t_Saught, raise ValueError
# If given 2 solutions, use 'SLERP' Interpolation (Linear Spline)
# If given 3+ solutions, use 'SQUAD' Inteprolation (~Cubic Spline)
# If t_Saught is of zero-length, return empty array.
# for t_Saught values outside the t_Given range will be given as Zero-Quaternions. (Null-rotations)
# the t_Given and t_Saught should be floats of any type, though I'd suggest 'seconds into day' or 'Unix Time' or 'MET'.
# the q_Given should be a Nx4 numpy array.
########
# Returns a Nx4 np-array of floats, where N=len(t_Saught),
# of scalar-first quaternions.
def Quaternion_Interpolation(t_Given, q_Given, t_Saught):
q_Given = _Sanitize_Quaternions(q_Given)
# Sanity-Check: Pre-processing.
# Remove any Zero-Quaternions from the input-list.
t_Given = t_Given[~_Is_Zero_Quat(q_Given)]
q_Given = q_Given[~_Is_Zero_Quat(q_Given)]
# Short circuit: Given or seeking no solutions:
if len(q_Given) == 0:
return qZero(len(t_Saught))
if len(t_Saught) == 0:
return qZero(len(t_Saught))
# Short circuit: Seeking the only given solution:
if len(q_Given) == 1 and t_Given[0] == t_Saught[0]:
return q_Given
# Make sure we're not trying to interpolate a single value.
if len(q_Given) == 1 and len(t_Saught) != 1:
raise ValueError("Trying to interpolate a single non-null quaternion into a range!")
if len(q_Given) == 1 and t_Given[0] != t_Saught[0]:
raise ValueError("Trying to interpolate a single non-null quaternion into another time!")
# And finally, interpolate the quaternions.
if len(q_Given) == 2:
return _SLERP_Interpolation_Setup(t_Given, q_Given, t_Saught)
if len(q_Given) >= 3:
return _SQUAD_Interpolation_Setup(t_Given, q_Given, t_Saught)
##################################################
########## Quaternion Averaging/Merging ##########
def Average_Quaternions( q ):
'''
Given a small list of quaternions, each describing the same rotation,
Return the average solution, as defined by their equal weights.
Note: This is only accurate for small-angle corrections.
Suggest keeping >5* total divergence.
Methods:
len=2,4 : SLERP
len=3,5+: Weighted Quaternion Averaging.
https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/20070017872.pdf
https://stackoverflow.com/questions/12374087/average-of-multiple-quaternions
https://www.mathworks.com/matlabcentral/fileexchange/40098-tolgabirdal-averaging_quaternions
Args:
q - Nx4 nd.array of floats - Quaternions, any frame.
Returns:
q - 1x4 nd.array of floats - The averaged Quaternion.
'''
# Sanitize input:
q = _Prep_For_Interpolation( q ) # Sanitizes, Normalizes, aligns the Quaternions.
if len(q) == 0: return qZero(0) # if given nothing, return nothing.
if len(q) == 1: return q # if given only one, return that one.
# If only given two, return the midpoint using SLERP.
if len(q) == 2:
Left, Right = q[:1], q[1:]
return _SLERP( Left, Right, [0.5] )
# If given 4, apply the SLERP twice.
elif len(q) == 4:
L1,L2,R1,R2 = q[0:1], q[1:2], q[2:3], q[3:4]
Left = _SLERP( L1, L2, [0.5] )
Right = _SLERP( R1, R2, [0.5] )
return _SLERP( Left, Right, [0.5] )
# If given any other length, use weighted Quaternion Averaging.
# TLDR: <INCOHERENT SCREAMING>
# Sum the quaternion outer products, divide by their total weight, return the column with the greatest eigenvalue.
# Make sure all quaternion scalar entries are positive. Not really sure why.
q[ q[:,0] < 0 ] *= -1
# For each quaternion, calculate it's outer product - A 4x4 matrix == (q . q.T)s
Outer_Product = numpy.array( [numpy.outer( [q[i]], [q[i]] ) for i in range(len(q))] ) # Nx4x4 array
# Then sum the outer products, and apply an equal weighting. (Divide by len(q))
Average_Outer_Matrix = numpy.sum(Outer_Product, axis=0) / len(q) # 4x4 array.
# And now we have an 'averaging matrix' for the quaternions.
# Extract the eigenvalues & vectors for the 4x4 'averaging matrix'
Eigen_Values, Eigen_Vectors = linalg.eig( Average_Outer_Matrix )
# Discard the imaginary components from the Eigenvalues: (All the imaginary components are guaranteed zero)
Eigen_Values = numpy.real(Eigen_Values)
# And the largest eigenvalue corresponds to the best average. Usually it's index [0] when values are close to Null.
Index_Of_Max_Eigenvalue = numpy.where( Eigen_Values == numpy.max(Eigen_Values ))[0][0]
# And want to pull the Eigenvector -column- cooresponding to that maximum eigenvalue.
Merged_Quaternion = Eigen_Vectors[:, Index_Of_Max_Eigenvalue ]
# Working in Eigenvector space -can- (but rarely) create complex values, but the should all zero. Only keep the Real component.
Merged_Quaternion = numpy.real( Merged_Quaternion )
# And reshape to the promised (1,4) before returning.
Merged_Quaternion = Merged_Quaternion.reshape(1,4)
return Merged_Quaternion
################################################
########## Secondary Frame Derivation ##########
def J2K_Ephemeris_to_J2K_to_Nadir_Quaternions( J2K_Ephemeris_Data ):
'''
From J2K ephemeris data, as produced by ephemeris.get_j2k_ephemeris,
Derive and return the qJ2K->Nadir quaternions for the given period.
Does not consider or require the timestamps.
Args:
J2K_Ephemeris_Data: Structrued Array of ephemeris data. Only considers j2k_[x|y|z|vx|vy|vz] columns
Returns:
nd.array, Nx4, of scalar-first quaternions describing the Nadir<-J2K rotation.
'''
from numpy.lib import recfunctions
Required_Columns = ['j2k_x','j2k_y','j2k_z','j2k_vx','j2k_vy','j2k_vz']
# Check to make sure all the requried columns are present:
if any([ needed_column not in J2K_Ephemeris_Data.dtype.names for needed_column in Required_Columns ]):
raise ValueError("Given data object does not contain one of the requisite 'j2k_x/y/z/vx/vy/vz' columns!")
# Translate the Strucutred array into a Nx6 array of cartesian ephemeris data:
J2K_Ephemeris_Data = recfunctions.structured_to_unstructured( J2K_Ephemeris_Data[Required_Columns] ) # Also strips the MET & other columns.
# Split the Nx6 into 2 Nx3, position and velocity:
Position = J2K_Ephemeris_Data[:, :3]
Velocity = J2K_Ephemeris_Data[:, 3:]
zVector = -Position # Invert vector, as Z is satellite-down rather than center-of-earth-up as given in the position.
yVector = numpy.cross(zVector, Velocity)
xVector = numpy.cross(yVector, zVector)
# Normalize the vectors:
xVector = Normalize(xVector)
yVector = Normalize(yVector)
zVector = Normalize(zVector)
# numpy.dstack stacks side-by-side, instead of numpy.array's stack, annoyingly.
# But it's still convenient for rotation-matricies.
# Build the rotation matrix, which due to the side-by-side, is Nadir->J2K
rNadir_to_J2K = numpy.dstack([xVector, yVector, zVector])
# Translate this into quaternions:
qNadir_to_J2K = Rotation_Matrix_to_Quaternion( rNadir_to_J2K )
# And conjugate to get the qNadir<-J2K desired.
qJ2K_to_Nadir = Conjugate( qNadir_to_J2K )
return qJ2K_to_Nadir
def GEO_Ephemeris_to_GEO_to_NEC_Quaternions( GEO_Ephemeris_Data ):
'''
From GEO ephemeris data, as produced by ephemeris.get_geo_ephemeris,
Derive and return the qITRF->NEC quaternions for the given period.
Derivation Source:
https://earth.esa.int/documents/10174/1514862/Swarm_L1b_Product_Definition - Page 61
Args:
GEO_Ephemeris_Data: Structrued Array of ephemeris data. Only considers geo_[x|y|z] columns
Returns:
nd.array, Nx4, of scalar-first right-handed quaternions describing the NEC<-GEO rotation.
'''
from numpy.lib import recfunctions
Position_Column_Names = ['geo_x','geo_y','geo_z']
# Check to make sure all the requried columns are present:
if any([ needed_column not in GEO_Ephemeris_Data.dtype.names for needed_column in Position_Column_Names ]):
raise ValueError("Given data object does not contain one of the requisite 'j2k_x/y/z/vx/vy/vz' columns!")
# Translate the Strucutred array into a Nx3 array of cartesian position data:
Position = recfunctions.structured_to_unstructured( GEO_Ephemeris_Data[Position_Column_Names] ) # Also strips the MET & other columns.
# Derive the raw vectors:
vCenter = -Position
vEast = numpy.cross( vCenter, [0,0,1.] ) # The cross of the 'Center' and an ideal ITRF+Z vector.
vNorth = numpy.cross( vEast, vCenter )
# And normalize:
vNorth = Normalize(vNorth )
vEast = Normalize(vEast )
vCenter = Normalize(vCenter)
# numpy.dstack stacks side-by-side, annoyingly.
# But it's still convenient for rotation-matricies, just producing the transpose of a vertical-stack.
# Build the rotation matrix, which due to the side-by-side, is GEO<-NEC
rNEC_to_ITRF = numpy.dstack([vNorth, vEast, vCenter])
# Translate this into quaternions:
qNEC_to_ITRF = Rotation_Matrix_to_Quaternion( rNEC_to_ITRF )
# And conjugate to get the qITRF_to_NEC desired.
qITRF_to_NEC = Conjugate( qNEC_to_ITRF )
return qITRF_to_NEC
Functions
def Average_Quaternions(q)
-
Given a small list of quaternions, each describing the same rotation, Return the average solution, as defined by their equal weights. Note: This is only accurate for small-angle corrections. Suggest keeping >5* total divergence.
Methods: len=2,4 : SLERP len=3,5+: Weighted Quaternion Averaging. https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/20070017872.pdf https://stackoverflow.com/questions/12374087/average-of-multiple-quaternions https://www.mathworks.com/matlabcentral/fileexchange/40098-tolgabirdal-averaging_quaternions
Args
q - Nx4 nd.array of floats - Quaternions, any frame.
Returns
q - 1x4 nd.array of floats - The averaged Quaternion.
Expand source code
def Average_Quaternions( q ): ''' Given a small list of quaternions, each describing the same rotation, Return the average solution, as defined by their equal weights. Note: This is only accurate for small-angle corrections. Suggest keeping >5* total divergence. Methods: len=2,4 : SLERP len=3,5+: Weighted Quaternion Averaging. https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/20070017872.pdf https://stackoverflow.com/questions/12374087/average-of-multiple-quaternions https://www.mathworks.com/matlabcentral/fileexchange/40098-tolgabirdal-averaging_quaternions Args: q - Nx4 nd.array of floats - Quaternions, any frame. Returns: q - 1x4 nd.array of floats - The averaged Quaternion. ''' # Sanitize input: q = _Prep_For_Interpolation( q ) # Sanitizes, Normalizes, aligns the Quaternions. if len(q) == 0: return qZero(0) # if given nothing, return nothing. if len(q) == 1: return q # if given only one, return that one. # If only given two, return the midpoint using SLERP. if len(q) == 2: Left, Right = q[:1], q[1:] return _SLERP( Left, Right, [0.5] ) # If given 4, apply the SLERP twice. elif len(q) == 4: L1,L2,R1,R2 = q[0:1], q[1:2], q[2:3], q[3:4] Left = _SLERP( L1, L2, [0.5] ) Right = _SLERP( R1, R2, [0.5] ) return _SLERP( Left, Right, [0.5] ) # If given any other length, use weighted Quaternion Averaging. # TLDR: <INCOHERENT SCREAMING> # Sum the quaternion outer products, divide by their total weight, return the column with the greatest eigenvalue. # Make sure all quaternion scalar entries are positive. Not really sure why. q[ q[:,0] < 0 ] *= -1 # For each quaternion, calculate it's outer product - A 4x4 matrix == (q . q.T)s Outer_Product = numpy.array( [numpy.outer( [q[i]], [q[i]] ) for i in range(len(q))] ) # Nx4x4 array # Then sum the outer products, and apply an equal weighting. (Divide by len(q)) Average_Outer_Matrix = numpy.sum(Outer_Product, axis=0) / len(q) # 4x4 array. # And now we have an 'averaging matrix' for the quaternions. # Extract the eigenvalues & vectors for the 4x4 'averaging matrix' Eigen_Values, Eigen_Vectors = linalg.eig( Average_Outer_Matrix ) # Discard the imaginary components from the Eigenvalues: (All the imaginary components are guaranteed zero) Eigen_Values = numpy.real(Eigen_Values) # And the largest eigenvalue corresponds to the best average. Usually it's index [0] when values are close to Null. Index_Of_Max_Eigenvalue = numpy.where( Eigen_Values == numpy.max(Eigen_Values ))[0][0] # And want to pull the Eigenvector -column- cooresponding to that maximum eigenvalue. Merged_Quaternion = Eigen_Vectors[:, Index_Of_Max_Eigenvalue ] # Working in Eigenvector space -can- (but rarely) create complex values, but the should all zero. Only keep the Real component. Merged_Quaternion = numpy.real( Merged_Quaternion ) # And reshape to the promised (1,4) before returning. Merged_Quaternion = Merged_Quaternion.reshape(1,4) return Merged_Quaternion
def Conjugate(q)
-
Inverts the quaternion, s.t. it now describes the opposite rotation. Does so by inverting the vector component, as negating the scalar has edge cases in it's use.
Expand source code
def Conjugate(q): ''' Inverts the quaternion, s.t. it now describes the opposite rotation. Does so by inverting the vector component, as negating the scalar has edge cases in it's use. ''' return q * [1, -1, -1, -1]
def GEO_Ephemeris_to_GEO_to_NEC_Quaternions(GEO_Ephemeris_Data)
-
From GEO ephemeris data, as produced by ephemeris.get_geo_ephemeris, Derive and return the qITRF->NEC quaternions for the given period.
Derivation Source: https://earth.esa.int/documents/10174/1514862/Swarm_L1b_Product_Definition - Page 61
Args
GEO_Ephemeris_Data
- Structrued Array of ephemeris data. Only considers geo_[x|y|z] columns
Returns
nd.array, Nx4, of scalar-first right-handed quaternions describing the NEC<-GEO rotation.
Expand source code
def GEO_Ephemeris_to_GEO_to_NEC_Quaternions( GEO_Ephemeris_Data ): ''' From GEO ephemeris data, as produced by ephemeris.get_geo_ephemeris, Derive and return the qITRF->NEC quaternions for the given period. Derivation Source: https://earth.esa.int/documents/10174/1514862/Swarm_L1b_Product_Definition - Page 61 Args: GEO_Ephemeris_Data: Structrued Array of ephemeris data. Only considers geo_[x|y|z] columns Returns: nd.array, Nx4, of scalar-first right-handed quaternions describing the NEC<-GEO rotation. ''' from numpy.lib import recfunctions Position_Column_Names = ['geo_x','geo_y','geo_z'] # Check to make sure all the requried columns are present: if any([ needed_column not in GEO_Ephemeris_Data.dtype.names for needed_column in Position_Column_Names ]): raise ValueError("Given data object does not contain one of the requisite 'j2k_x/y/z/vx/vy/vz' columns!") # Translate the Strucutred array into a Nx3 array of cartesian position data: Position = recfunctions.structured_to_unstructured( GEO_Ephemeris_Data[Position_Column_Names] ) # Also strips the MET & other columns. # Derive the raw vectors: vCenter = -Position vEast = numpy.cross( vCenter, [0,0,1.] ) # The cross of the 'Center' and an ideal ITRF+Z vector. vNorth = numpy.cross( vEast, vCenter ) # And normalize: vNorth = Normalize(vNorth ) vEast = Normalize(vEast ) vCenter = Normalize(vCenter) # numpy.dstack stacks side-by-side, annoyingly. # But it's still convenient for rotation-matricies, just producing the transpose of a vertical-stack. # Build the rotation matrix, which due to the side-by-side, is GEO<-NEC rNEC_to_ITRF = numpy.dstack([vNorth, vEast, vCenter]) # Translate this into quaternions: qNEC_to_ITRF = Rotation_Matrix_to_Quaternion( rNEC_to_ITRF ) # And conjugate to get the qITRF_to_NEC desired. qITRF_to_NEC = Conjugate( qNEC_to_ITRF ) return qITRF_to_NEC
def Geodesic_Norm(A, B)
-
Given two quaternions, return the total angle in degrees between them. Must be of equal shape, describing the same rotation. Can provide a Null Quaternion [1,0,0,0] as B to get the total rotation of A
Expand source code
def Geodesic_Norm(A, B): ''' Given two quaternions, return the total angle in degrees between them. Must be of equal shape, describing the same rotation. Can provide a Null Quaternion [1,0,0,0] as B to get the total rotation of A ''' A = Normalize( _Sanitize_Quaternions(A) ) B = Normalize( _Sanitize_Quaternions(B) ) if A.shape != B.shape: raise ValueError("Geodesic Norm given sets of unequal shape!") Dot = numpy.sum(A * B, axis=1) Dot[Dot < -1.0] = -1.0 Dot[Dot > 1.0] = 1.0 return numpy.arccos(2 * Dot**2 - 1) * 180 / numpy.pi
def J2K_Ephemeris_to_J2K_to_Nadir_Quaternions(J2K_Ephemeris_Data)
-
From J2K ephemeris data, as produced by ephemeris.get_j2k_ephemeris, Derive and return the qJ2K->Nadir quaternions for the given period. Does not consider or require the timestamps.
Args
J2K_Ephemeris_Data
- Structrued Array of ephemeris data. Only considers j2k_[x|y|z|vx|vy|vz] columns
Returns
nd.array, Nx4, of scalar-first quaternions describing the Nadir<-J2K rotation.
Expand source code
def J2K_Ephemeris_to_J2K_to_Nadir_Quaternions( J2K_Ephemeris_Data ): ''' From J2K ephemeris data, as produced by ephemeris.get_j2k_ephemeris, Derive and return the qJ2K->Nadir quaternions for the given period. Does not consider or require the timestamps. Args: J2K_Ephemeris_Data: Structrued Array of ephemeris data. Only considers j2k_[x|y|z|vx|vy|vz] columns Returns: nd.array, Nx4, of scalar-first quaternions describing the Nadir<-J2K rotation. ''' from numpy.lib import recfunctions Required_Columns = ['j2k_x','j2k_y','j2k_z','j2k_vx','j2k_vy','j2k_vz'] # Check to make sure all the requried columns are present: if any([ needed_column not in J2K_Ephemeris_Data.dtype.names for needed_column in Required_Columns ]): raise ValueError("Given data object does not contain one of the requisite 'j2k_x/y/z/vx/vy/vz' columns!") # Translate the Strucutred array into a Nx6 array of cartesian ephemeris data: J2K_Ephemeris_Data = recfunctions.structured_to_unstructured( J2K_Ephemeris_Data[Required_Columns] ) # Also strips the MET & other columns. # Split the Nx6 into 2 Nx3, position and velocity: Position = J2K_Ephemeris_Data[:, :3] Velocity = J2K_Ephemeris_Data[:, 3:] zVector = -Position # Invert vector, as Z is satellite-down rather than center-of-earth-up as given in the position. yVector = numpy.cross(zVector, Velocity) xVector = numpy.cross(yVector, zVector) # Normalize the vectors: xVector = Normalize(xVector) yVector = Normalize(yVector) zVector = Normalize(zVector) # numpy.dstack stacks side-by-side, instead of numpy.array's stack, annoyingly. # But it's still convenient for rotation-matricies. # Build the rotation matrix, which due to the side-by-side, is Nadir->J2K rNadir_to_J2K = numpy.dstack([xVector, yVector, zVector]) # Translate this into quaternions: qNadir_to_J2K = Rotation_Matrix_to_Quaternion( rNadir_to_J2K ) # And conjugate to get the qNadir<-J2K desired. qJ2K_to_Nadir = Conjugate( qNadir_to_J2K ) return qJ2K_to_Nadir
def Normalize(q)
-
Given an array of floats, normalize them to a length of 1.0 Valid for Quaternions and Vectors.
Args
q
- NxM np-array of floats.
Returns
qN
- NxM np-array of floats, where each row has a norm/length of 1.0
Expand source code
def Normalize(q): ''' Given an array of floats, normalize them to a length of 1.0 Valid for Quaternions and Vectors. Args: q: NxM np-array of floats. Returns: qN: NxM np-array of floats, where each row has a norm/length of 1.0 ''' if len(q) == 0: # If empty, return empty. return q q_Norms = numpy.linalg.norm(q, axis=1) # Computes vector lengths. q_Norms[q_Norms <= 1e-10] = 1 # If given a 'Zero' Quaternion, ~[0,0,0,0], just divide by 1 to avoid Divide-by-zero errors. q_Normalized = (q.T / q_Norms).T # Quirk of strucure & broadcasting rules, but does the division correctly. return q_Normalized
def Quaternion_Interpolation(t_Given, q_Given, t_Saught)
-
Expand source code
def Quaternion_Interpolation(t_Given, q_Given, t_Saught): q_Given = _Sanitize_Quaternions(q_Given) # Sanity-Check: Pre-processing. # Remove any Zero-Quaternions from the input-list. t_Given = t_Given[~_Is_Zero_Quat(q_Given)] q_Given = q_Given[~_Is_Zero_Quat(q_Given)] # Short circuit: Given or seeking no solutions: if len(q_Given) == 0: return qZero(len(t_Saught)) if len(t_Saught) == 0: return qZero(len(t_Saught)) # Short circuit: Seeking the only given solution: if len(q_Given) == 1 and t_Given[0] == t_Saught[0]: return q_Given # Make sure we're not trying to interpolate a single value. if len(q_Given) == 1 and len(t_Saught) != 1: raise ValueError("Trying to interpolate a single non-null quaternion into a range!") if len(q_Given) == 1 and t_Given[0] != t_Saught[0]: raise ValueError("Trying to interpolate a single non-null quaternion into another time!") # And finally, interpolate the quaternions. if len(q_Given) == 2: return _SLERP_Interpolation_Setup(t_Given, q_Given, t_Saught) if len(q_Given) >= 3: return _SQUAD_Interpolation_Setup(t_Given, q_Given, t_Saught)
def Quaternion_to_Rotation_Matrix(q)
-
Given a list of quaternions, return their equivalent rotation matricies.
Given quaternions must be RIGHT-HANDED - vB = qB<-A. vA . qB<-A = q.v.q Returned rotation matricies are LEFT-HANDED - vB = rB<-A . vA
Expand source code
def Quaternion_to_Rotation_Matrix(q): ''' Given a list of quaternions, return their equivalent rotation matricies. #### Given quaternions must be RIGHT-HANDED - vB = qB<-A*. vA . qB<-A = q*.v.q Returned rotation matricies are LEFT-HANDED - vB = rB<-A . vA ''' # Sanity-checking. q = _Sanitize_Quaternions(q) q = Normalize(q) qR, qI, qJ, qK = [q[:, i] for i in range(4)] # Splits the array (either single or bulk) into the individual columns. A = 1 - 2 * (qJ**2 + qK**2) B = 2 * (qI * qJ + qK * qR) C = 2 * (qI * qK - qJ * qR) D = 2 * (qI * qJ - qK * qR) #[[A,B,C], E = 1 - 2 * (qI**2 + qK**2) # [D,E,F], F = 2 * (qJ * qK + qI * qR) # [G,H,I]] G = 2 * (qI * qK + qJ * qR) H = 2 * (qJ * qK - qI * qR) I = 1 - 2 * (qI**2 + qJ**2) return numpy.dstack(( A,B,C, D,E,F, G,H,I )).reshape(-1, 3, 3)
def Quaternion_to_YPR(qNadir_to_CRF)
-
Given a list of quaternions, Return an Nx3 array of Yaw, Pitch, and Roll, in degrees. To match the CAS_Ephemeris YPR data, must be given Nadir->CRF/Body quaternions.
Expand source code
def Quaternion_to_YPR( qNadir_to_CRF ): ''' Given a list of quaternions, Return an Nx3 array of Yaw, Pitch, and Roll, in degrees. To match the CAS_Ephemeris YPR data, must be given Nadir->CRF/Body quaternions. ''' qNadir_to_CRF = Normalize(_Sanitize_Quaternions(qNadir_to_CRF)) # Note that below derivation uses the 123/XYZ rotation-derivation. # Derivation of the 321/ZYX is done by inverting the input quaternion (Body->Nadir), and negating the YPR values. q = Conjugate(qNadir_to_CRF) qR, qI, qJ, qK = q.T # Splie the Quatenions Column-wise Roll = -numpy.arctan2(2 * (qR * qI + qJ * qK), 1 - 2 * (qI**2 + qJ**2)) Pitch = -numpy.arcsin (2 * (qR * qJ - qK * qI)) Yaw = -numpy.arctan2(2 * (qR * qK + qI * qJ), 1 - 2 * (qJ**2 + qK**2)) # Note that in the event of a Pitch=+-90*, within a few arcseconds, the Pitch value will return NaN, as this is a Gimble Lock. # As a workaround, if any row has a NaN, then replace all row with 999/999/999 YPR = numpy.column_stack((Yaw, Pitch, Roll)) * 180 / numpy.pi YPR[numpy.any(numpy.isnan(YPR), axis=1)] = [999, 999, 999] return YPR
def Rotation_Matrix_to_Quaternion(R)
-
Given arrays of rotation matricies, return their equivalent quaternion.
Rotation matricies must be LEFT-HANDED - vB = rB<-A . vA Returned quaternions are RIGHT-HANDED - vB = qB<-A. vA . qB<-A = q.v.q
Takes arrays of either Nx3x3 or Nx1x9. 3x3 matrix == 1x9 matrix of the following format: [A B C] [D E F] == [A B C D E F G H I] [G H I]
Example Calculation: qR = 0.5 * (1+ A+E+I)0.5 qI = (H-F) / 4qR qJ = (C-G) / 4qR qK = (D-B) / 4*qR But the divisor entry (qR here) needs to be the largest absolute entry as to avoid division error. Can calculate qI, qJ, or qJ to also be the divisor.
Expand source code
def Rotation_Matrix_to_Quaternion(R): ''' Given arrays of rotation matricies, return their equivalent quaternion. #### Rotation matricies must be LEFT-HANDED - vB = rB<-A . vA Returned quaternions are RIGHT-HANDED - vB = qB<-A*. vA . qB<-A = q*.v.q #### Takes arrays of either Nx3x3 or Nx1x9. 3x3 matrix == 1x9 matrix of the following format: [A B C] [D E F] == [A B C D E F G H I] [G H I] ### Example Calculation: qR = 0.5 * (1+ A+E+I)**0.5 qI = (H-F) / 4*qR qJ = (C-G) / 4*qR qK = (D-B) / 4*qR But the divisor entry (qR here) needs to be the largest absolute entry as to avoid division error. Can calculate qI, qJ, or qJ to also be the divisor. ''' R = numpy.array(R) # Instant if already in nd.array if R.shape[1:] not in [(3, 3), (9,)]: raise ValueError("Must be fed either an Nx3x3 or an Nx9 array") # If linear array, reshape to 3x3. if R.shape[1:] == (9,): R = R.reshape(len(R), 3, 3) # Rename the solution entries. A = R[:, 0, 0] # [A B C] B = R[:, 0, 1] # R->[D E F] C = R[:, 0, 2] # [G H I] D = R[:, 1, 0] E = R[:, 1, 1] F = R[:, 1, 2] G = R[:, 2, 0] H = R[:, 2, 1] I = R[:, 2, 2] # Calculate the basis entires. Guaranteed positive. # AEI == Trace, 3x3 rot-matrix trace range {-1->3}. # Sanity check against the other variants inside the sqrt all prove positive. # Though -CAN- have negative with floating point error. The numpy.abs is just for sanity, as the negative value will never be used as the basis. qR_Basis = 0.5 * numpy.abs(1 + A + E + I)**0.5 qI_Basis = 0.5 * numpy.abs(1 + A - E - I)**0.5 qJ_Basis = 0.5 * numpy.abs(1 - A + E - I)**0.5 qK_Basis = 0.5 * numpy.abs(1 - A - E + I)**0.5 # Build, then extract the best 'basis' entries to be used for the division of each solution. Basis_Array = numpy.column_stack((qR_Basis, qI_Basis, qJ_Basis, qK_Basis)) # Builds a mask where the row's max index is this index. qR_Basis_Mask = numpy.max(Basis_Array, axis=1) == Basis_Array[:, 0] qI_Basis_Mask = numpy.max(Basis_Array, axis=1) == Basis_Array[:, 1] # Use this as the divisor, to minimize error. qJ_Basis_Mask = numpy.max(Basis_Array, axis=1) == Basis_Array[:, 2] # In the event of A == B, just overwrites. # No need to test sign, as Basis Array guaranteed positive. qK_Basis_Mask = numpy.max(Basis_Array, axis=1) == Basis_Array[:, 3] # Build the relevant rotation-matrix entries. qRqI = qIqR = (F - H) / 4 qRqJ = qJqR = (G - C) / 4 qRqK = qKqR = (B - D) / 4 qJqK = qKqJ = (F + H) / 4 qIqK = qKqI = (G + C) / 4 qIqJ = qJqI = (B + D) / 4 # Pre-initialize the array. Q = numpy.zeros((len(A), 4)) # For each entry where the Basis is the greatest, calculate the other components. if len(qR_Basis_Mask) > 0: Q[qR_Basis_Mask, 0] = qR_Basis[qR_Basis_Mask] Q[qR_Basis_Mask, 1] = qIqR[qR_Basis_Mask] / qR_Basis[qR_Basis_Mask] Q[qR_Basis_Mask, 2] = qJqR[qR_Basis_Mask] / qR_Basis[qR_Basis_Mask] Q[qR_Basis_Mask, 3] = qKqR[qR_Basis_Mask] / qR_Basis[qR_Basis_Mask] if len(qI_Basis_Mask) > 0: Q[qI_Basis_Mask, 0] = qRqI[qI_Basis_Mask] / qI_Basis[qI_Basis_Mask] Q[qI_Basis_Mask, 1] = qI_Basis[qI_Basis_Mask] Q[qI_Basis_Mask, 2] = qJqI[qI_Basis_Mask] / qI_Basis[qI_Basis_Mask] Q[qI_Basis_Mask, 3] = qKqI[qI_Basis_Mask] / qI_Basis[qI_Basis_Mask] if len(qJ_Basis_Mask) > 0: Q[qJ_Basis_Mask, 0] = qRqJ[qJ_Basis_Mask] / qJ_Basis[qJ_Basis_Mask] Q[qJ_Basis_Mask, 1] = qIqJ[qJ_Basis_Mask] / qJ_Basis[qJ_Basis_Mask] Q[qJ_Basis_Mask, 2] = qJ_Basis[qJ_Basis_Mask] Q[qJ_Basis_Mask, 3] = qKqJ[qJ_Basis_Mask] / qJ_Basis[qJ_Basis_Mask] if len(qK_Basis_Mask) > 0: Q[qK_Basis_Mask, 0] = qRqK[qK_Basis_Mask] / qK_Basis[qK_Basis_Mask] Q[qK_Basis_Mask, 1] = qIqK[qK_Basis_Mask] / qK_Basis[qK_Basis_Mask] Q[qK_Basis_Mask, 2] = qJqK[qK_Basis_Mask] / qK_Basis[qK_Basis_Mask] Q[qK_Basis_Mask, 3] = qK_Basis[qK_Basis_Mask] return Normalize(Q) # Final sanity check.
def Vector_Rotate(qA_to_B, vA)
-
Given quaternions, and a list of vectors in the 'source' frame, Return the vector in the 'destination' frame.
Rotation for this is q.v.q, using Right-Hand quaternions. [0,vB] = qB<-A . [0,vA] . qB<-A = qA<-B . [0,vA] . qB<-A # Left-arrow notation. = qB->A . [0,vA] . qA->B # Right-arrow notation.
Equivalent to the left-Handed rotation-matrix rotation of vB = Rot(A->B) . vA
Args
qA_to_B
- Quaternion list of floats expressing the rotation from frame 'A' into frame 'B'. Expressed as 'qA->B' or 'qB<-A'
vA
- A single or list of 3-vectors of floats that expresses a vector in frame 'A'.
Returns
vB
- A list of 3-vectors of floats that expresses vectors in frame 'B'.
Expand source code
def Vector_Rotate( qA_to_B, vA ): ''' Given quaternions, and a list of vectors in the 'source' frame, Return the vector in the 'destination' frame. Rotation for this is q*.v.q, using Right-Hand quaternions. [0,vB] = qB<-A* . [0,vA] . qB<-A = qA<-B . [0,vA] . qB<-A # Left-arrow notation. = qB->A . [0,vA] . qA->B # Right-arrow notation. Equivalent to the left-Handed rotation-matrix rotation of vB = Rot(A->B) . vA Args: qA_to_B: Quaternion list of floats expressing the rotation from frame 'A' into frame 'B'. Expressed as 'qA->B' or 'qB<-A' vA: A single or list of 3-vectors of floats that expresses a vector in frame 'A'. Returns: vB: A list of 3-vectors of floats that expresses vectors in frame 'B'. ''' qA_to_B = _Sanitize_Quaternions( qA_to_B ) q = qA_to_B qC = qB_to_A = Conjugate(qA_to_B) # And it's conjugate. # And do a coarse sanitization of the vectors: vA = numpy.array(vA, dtype=float).reshape(-1,3) # Prepend a zero to the Vector to turn it into a zero-scalar quaternion. qVec_A = numpy.column_stack((numpy.zeros(len( vA )), vA )) # [0, v] qVec_A = _Sanitize_Quaternions( qVec_A ) # And then sanitize to check input shaping. # If either is empty, return an empty quaternion array. if len( qA_to_B ) == 0 or len(vA) == 0: return qZero(0) # Returns Zero-Vectors, empty. # Apply the rotation qVec_B = _qChain_Multiply(( qC, qVec_A, q )) # And then remove the zero-scalar value. vB = qVec_B[:, 1:] return vB
def YPR_to_Quaternion(YPR)
-
Given an Nx3 list of Yaw, Pitch, and Roll, return a rotation describing that rotation as a quaternion. If given the YPR as described by the CAS_ephemeris file, return Nadir->CRF/Body quaternions. The Inverse of the .Quaternion_to_YPR() function.
Expand source code
def YPR_to_Quaternion( YPR ): # NEEDS TESTING ''' Given an Nx3 list of Yaw, Pitch, and Roll, return a rotation describing that rotation as a quaternion. If given the YPR as described by the CAS_ephemeris file, return Nadir->CRF/Body quaternions. The Inverse of the .Quaternion_to_YPR() function. ''' # NOTE: Uses the ZYX/321 rotation structure, not the 123/XYZ Yaw, Pitch, Roll = -(YPR * numpy.pi / 180).T # Extract the dataset, in radians # The above values are negated, as the below derives the 123/XYZ rotation. # This is then inverted at the final step as to generate the 321/ZYX rotation. # Shortcuts: the sin/cos of half the angles. cY, sY = numpy.cos(Yaw / 2), numpy.sin(Yaw / 2) cP, sP = numpy.cos(Pitch / 2), numpy.sin(Pitch / 2) cR, sR = numpy.cos(Roll / 2), numpy.sin(Roll / 2) qR = cR * cP * cY + sR * sP * sY qI = sR * cP * cY - cR * sP * sY qJ = cR * sP * cY + sR * cP * sY qK = cR * cP * sY - sR * sP * cY Q = numpy.column_stack((qR, qI, qJ, qK)) return Conjugate(Q)
def qNull(Length=0)
-
Expand source code
def qNull(Length=0): # Builds array of null-quaternions. return numpy.full((Length, 4), NULL_QUAT, dtype=float)
def qZero(Length=0)
-
Expand source code
def qZero(Length=0): # Builds array of zero-quaternions. return numpy.full((Length, 4), ZERO_QUAT, dtype=float)