Attitude Transformations¶

navpy.angle2quat(rotAngle1, rotAngle2, rotAngle3, input_unit='rad', rotation_sequence='ZYX')¶

Convert a sequence of rotation angles to an equivalent unit quaternion

This function can take inputs in either degree or radians, and can also batch process a series of rotations (e.g., time series of Euler angles). By default this function assumes aerospace rotation sequence but can be changed using the rotation_sequence keyword argument.

Parameters:

rotAngle1, rotAngle2, rotAngle3 : {(N,), (N,1), or (1,N)}

They are a sequence of angles about successive axes described by rotation_sequence.

input_unit : {‘rad’, ‘deg’}, optional

Rotation angles. Default is ‘rad’.

rotation_sequence : {‘ZYX’}, optional

Rotation sequences. Default is ‘ZYX’.

Returns:

q0 : {(N,)} array like scalar componenet of the quaternion

qvec : {(N,3)} array like vector component of the quaternion

Notes

Convert rotation angles to unit quaternion that transfroms a vector in F1 to F2 according to

$v_q^{F2} = q^{-1} \otimes v_q^{F1} \otimes q$

where $\otimes$ indicates the quaternion multiplcation and $v_q^F$ is a pure quaternion representation of the vector $v_q^F$ . The scalar componenet of $v_q^F$ is zero. For aerospace sequence (‘ZYX’): rotAngle1 = psi, rotAngle2 = the, and rotAngle3 = phi

Examples

>>> import numpy as np
>>> from navpy import angle2quat
>>> psi = 0
>>> theta = np.pi/4.0
>>> phi = np.pi/3.0
>>> q0, qvec = angle2quat(psi,theta,phi)
>>> q0
0.80010314519126557
>>> qvec
array([ 0.46193977,  0.33141357, -0.19134172])

>>> psi = [10, 20, 30]
>>> theta = [30, 40, 50]
>>> phi = [0, 5, 10]
>>> q0, qvec = angle2quat(psi,theta,phi,input_unit = 'deg')
>>> q0
array([ 0.96225019,  0.92712639,  0.88162808])
>>> qvec
array([[-0.02255757,  0.25783416,  0.08418598],
       [-0.01896854,  0.34362114,  0.14832854],
       [-0.03266701,  0.4271086 ,  0.19809857]])

navpy.dcm2quat(C, rotation_sequence='ZYX')¶

Convert a DCM to a unit quaternion

Parameters:

C : direction consine matrix that rotates the vector from the first frame

to the second frame according to the specified rotation_sequence. rotation_sequence: {‘ZYX’}, optional. Rotation sequences. Default is ‘ZYX’.

Returns:

q0 : {(N,)} array_like

Scalar componenet of the quaternion

qvec : {(N,3)} array_like

Vector component of the quaternion

Examples

>>> import numpy as np
>>> from navpy import dcm2quat
>>> C = np.array([[  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]])
>>> q0,qvec = dcm2quat(C)
>>> q0
0.98111933015306552
>>> qvec
array([-0.0150997 ,  0.08579831,  0.17299659])

navpy.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]]))

navpy.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 transfroms a vector in F1 to F2 according to

$v_q^{F2} = q^{-1} \otimes v_q^{F1} \otimes q$

where $\otimes$ indicates the quaternion multiplcation and $v_q^F$ is a pure quaternion representation of the vector $v_q^F$ . The scalar componenet of $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])

navpy.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]])

Utilities¶

navpy.wrapToPi(e)¶: Wraping angle to [-pi,pi] interval

navpy.omega2rates(pitch, roll, input_unit='rad', euler_angles_order='roll_pitch_yaw', output_type='ndarray')¶

This function is used to create the transformation matrix to go from:: [p, q, r] –> [roll_rate, pitch_rate, yaw_rate]

where pqr are xyz body rotation-rate measurements expressed in body frame. Yaw, pitch, and roll are the Euler angles. We assume the Euler angles are 3-2-1 (i.e Yaw -> Pitch -> Roll) transformations that go from navigation- frame to body-frame.

Parameters:

pitch : pitch angle, units of input_unit.

roll : roll angle , units of input_unit.

input_unit : units for input angles {‘rad’, ‘deg’}, optional

euler_angles_order : {‘roll_pitch_yaw’, ‘yaw_pitch_roll’}, optional

Assumed order of Euler Angles attitude state vector (see Notes).

output_type : {‘ndarray’ or ‘matrix’}, optional

Numpy array (default) or matrix

Returns:

R : transformation matrix, from xyz body-rate to Euler angle-rates

numpy ‘output_type’ 3x3 (Note: default return variable is an ARRAY, not a matrix)

Notes

Since the returned transformation matrix is used to transform one vector to another, the assumed attitude variables order matters. The euler_angles_order parameter can be used to specify the assumed order.

The difference is demonstrated by example:

By default euler_angles_order=’roll_pitch_yaw’ R = omega2rates(pitch, roll) [ roll_rate] [omega_x] [pitch_rate] = dot(R,[omega_y]) [ yaw_rate] [omega_z]

Now assume our attitude state is [yaw, pitch, roll].T R = omega2rates(pitch, roll, euler_angles_order=’yaw_pitch_roll’) [ yaw_rate] [omega_x] [pitch_rate] = dot(R,[omega_y]) [ roll_rate] [omega_z]

References

[1] Equation 2.74, Aided Navigation: GPS with High Rate Sensors,: Jay A. Farrel 2008

[2] omega2rates.m function at: http://www.gnssapplications.org/downloads/chapter7/Chapter7_GNSS_INS_Functions.tar.gz