TRIAD

The Tri-Axial Attitude Determination (TRIAD) was first described in [Black] to algebraically estimate an attitude represented as a Direction Cosine Matrix from two orthogonal vector observations.

Given two non-parallel reference unit vectors \(\mathbf{v}_1\) and \(\mathbf{v}_2\) and their corresponding unit vectors \(\mathbf{w}_1\) and \(\mathbf{w}_2\), it is required to find an orthogonal matrix \(\mathbf{A}\) satisfying:

\[\mathbf{Av}_1\]

Two vectors \(\mathbf{v}_1\) and \(\mathbf{v}_2\) define an orthogonal coordinate system with the normalized basis vectors \(\mathbf{q}\), \(\mathbf{r}\), and \(\mathbf{s}\) as the following triad:

\[\begin{split}\begin{array}{rcl} \mathbf{q}_r &=& \mathbf{v}_1 \\ \mathbf{r}_r &=& \frac{\mathbf{v}_1\times\mathbf{v}_2}{|\mathbf{v}_1\times\mathbf{v}_2|} \\ \mathbf{s}_r &=& \mathbf{q}_r\times\mathbf{r}_r \end{array}\end{split}\]

The TRIAD method, initially developed to estimate the attitude of spacecrafts [Shuster2007], uses the position of the sun (using a star tracker) and the magnetic field of Earth as references [Hall] [Makley]. These are represented as vectors to build an appropriate reference frame \(\mathbf{M}_r\):

\[\mathbf{M}_r = \begin{bmatrix} \mathbf{q}_r & \mathbf{r}_r & \mathbf{s}_r \end{bmatrix}\]

Similarly, at any given time, two measured vectors in the spacecraft’s body frame \(\mathbf{w}_1\) and \(\mathbf{w}_2\) determine the \(3\times 3\) body matrix \(\mathbf{M}_b\):

\[\mathbf{M}_b = \begin{bmatrix} \mathbf{q}_b & \mathbf{r}_b & \mathbf{s}_b \end{bmatrix}\]

where, like the first triad, the second triad is built as:

\[\begin{split}\begin{array}{rcl} \mathbf{q}_b &=& \mathbf{w}_1 \\ \mathbf{r}_b &=& \frac{\mathbf{w}_1\times\mathbf{w}_2}{|\mathbf{w}_1\times\mathbf{w}_2|} \\ \mathbf{s}_b &=& \mathbf{q}_b\times\mathbf{r}_b \end{array}\end{split}\]

The attitude matrix \(\mathbf{A}\in\mathbb{R}^{3\times 3}\) defines the coordinate transformation,

\[\mathbf{AM}_r = \mathbf{M}_b\]

Solving for \(\mathbf{A}\) we obtain:

\[\mathbf{A} = \mathbf{M}_b\mathbf{M}_r^{-1}\]

But we also know that \(\mathbf{M}_r\) is orthogonal. So, the solution is simply:

\[\mathbf{A} = \mathbf{M}_b\mathbf{M}_r^T\]

Inverse trigonometric functions are not required, a unique attitude is obtained, and computational requirements are minimal.

It is only required that \(\mathbf{M}_r\) has an inverse, but that is already ensured, since \(\mathbf{q}_r\), \(\mathbf{r}_r\), and \(\mathbf{s}_r\) are linearly independent [Lerner1].

Strapdown INS

For estimations using a Strapdown INS on Earth, we identify two main reference vectors: gravity \(\mathbf{g}=\begin{bmatrix}g_x & g_y & g_z\end{bmatrix}\) and magnetic field \(\mathbf{h}=\begin{bmatrix}h_x & h_y & h_z\end{bmatrix}\).

A common convention sets the gravity vector equal to \(0\) along the X- and Y-axis, and equal to \(\sim 9.81\) along the Z-axis. This assumes the direction of the gravity is parallel to the vertical axis. Because TRIAD uses normalized vectors, the Z-axis will turn out to be equal to \(1\):

\[\begin{split}\mathbf{g} = \begin{bmatrix}0 \\ 0 \\ 1 \end{bmatrix}\end{split}\]

The magnetic field is defined from the geographical position of the measurement. Using the World Magnetic Model, we can estimate the magnetic field elements of our location on a given date.

The class ahrs.utils.WMM can help us to retrieve it. Let’s say we want to know the geomagnetic field elements of Munich, Germany [1] on the 3rd of October, 2020.

The city’s location is 48.137154° N and 11.576124° E at 519 m above sea level. We obtain its magnetic elements as:

>>> import datetime
>>> from ahrs.utils import WMM
>>> wmm = WMM(latitude=48.137154, longitude=11.576124, height=0.519, date=datetime.date(2020, 10, 3))
>>> wmm.magnetic_elements
{'X': 21009.66924050522, 'Y': 1333.4601319284525, 'Z': 43731.849938722924, 'H': 21051.943319296533, 'F': 48535.13177670226, 'I': 64.2944417667441, 'D': 3.631627635223863, 'GV': 3.631627635223863}

For further explanation of class WMM, please check its page. Of our interest are only the values of X, Y and Z representing the magnetic field intensity, in nT, along the X-, Y- and Z-axis, respectively.

\[\begin{split}\mathbf{h} = \begin{bmatrix} 21009.66924 \\ 1333.46013 \\ 43731.84994 \end{bmatrix}\end{split}\]

But, again, TRIAD works with normalized vectors, so the reference magnetic vector becomes:

\[\begin{split}\mathbf{h} = \begin{bmatrix} 0.43288 \\ 0.02747 \\ 0.90103 \end{bmatrix}\end{split}\]
>>> import numpy as np
>>> h = np.array([wmm.magnetic_elements[x] for x in list('XYZ')])
>>> h /= np.linalg.norm(h)      # Reference geomagnetic field (h)
>>> h
array([0.4328755 , 0.02747412, 0.90103495])

Both normalized vectors \(\mathbf{g}\) and \(\mathbf{h}\) build the reference triad \(\mathbf{M}_r\)

Then, we have to measure their equivalent vectors, for which we use the accelerometer to obtain \(\mathbf{a} = \begin{bmatrix}a_x & a_y & a_z \end{bmatrix}\), and the magnetometer for \(\mathbf{m} = \begin{bmatrix}m_x & m_y & m_z \end{bmatrix}\).

Both measurement vectors are also normalized, meaning \(\|\mathbf{a}\|=\|\mathbf{m}\|=1\), so that they can build the body’s measurement triad \(\mathbf{M}_b\).

To get the Direction Cosine Matrix we simply call the method estimate with the normalized measurement vectors:

>>> triad = ahrs.filters.TRIAD()
>>> triad.v1 = np.array([0.0, 0.0, 1.0])                    # Reference gravity vector (g)
>>> triad.v2 = h                                            # Reference geomagnetic field (h)
>>> a = np.array([-2.499e-04, 4.739e-02, 0.9988763])        # Measured acceleration (normalized)
>>> a /= np.linalg.norm(a)
>>> m = np.array([-0.36663061, 0.17598138, -0.91357132])    # Measured magnetic field (normalized)
>>> m /= np.linalg.norm(m)
>>> triad.estimate(w1=a, w2=m)
array([[-8.48320410e-01, -5.29483162e-01, -2.49900033e-04],
       [ 5.28878238e-01, -8.47373587e-01,  4.73900062e-02],
       [-2.53039690e-02,  4.00697428e-02,  9.98876431e-01]])

Optionally, it can return the estimation as a quaternion representation setting as_quaternion to True.

>>> triad.estimate(w1=a, w2=m, as_quaternion=True)
array([ 0.27531002, -0.00664729,  0.02275078,  0.96106327])

Giving the observation vector to the constructor, the attitude estimation happens automatically, and is stored in the attribute A.

>>> triad = ahrs.filters.TRIAD(w1=np.array([-2.499e-04, 4.739e-02, 0.9988763]), w2=np.array([-0.36663061, 0.17598138, -0.91357132]), v2=h)
>>> triad.A
array([[-8.48320410e-01, -5.29483162e-01, -2.49900033e-04],
       [ 5.28878238e-01, -8.47373587e-01,  4.73900062e-02],
       [-2.53039690e-02,  4.00697428e-02,  9.98876431e-01]])
>>> triad = ahrs.filters.TRIAD(w1=np.array([-2.499e-04, 4.739e-02, 0.9988763]), w2=np.array([-0.36663061, 0.17598138, -0.91357132]), v2=h, as_quaternion=True)
>>> triad.A
array([ 0.27531002, -0.00664729,  0.02275078,  0.96106327])

If the input data contains many observations, all will be estimated at once.

>>> a = np.array([[-0.000249905733, 0.0473926177, 0.998876307],
... [-0.00480145530, 0.0572267567, 0.998349660],
... [-0.00986626329, 0.0746539896, 0.997160688]])
>>> m = np.array([[-0.36663061, 0.17598138, -0.91357132],
... [-0.37726367, 0.18069746, -0.90830642],
... [-0.3874741, 0.18536454, -0.9030525]])
>>> triad = ahrs.filters.TRIAD(w1=a, w2=m, v2=h)
>>> triad.A
array([[[-8.48317898e-01, -5.29487187e-01, -2.49905733e-04],
        [ 5.28882192e-01, -8.47370974e-01,  4.73926177e-02],
        [-2.53055467e-02,  4.00718352e-02,  9.98876307e-01]],

       [[-8.43678607e-01, -5.36827117e-01, -4.80145530e-03],
        [ 5.35721702e-01, -8.42453178e-01,  5.72267567e-02],
        [-3.47658761e-02,  4.57087466e-02,  9.98349660e-01]],

       [[-8.32771974e-01, -5.53528225e-01, -9.86626329e-03],
        [ 5.51396878e-01, -8.30896061e-01,  7.46539896e-02],
        [-4.95209297e-02,  5.67295235e-02,  9.97160688e-01]]])
>>> triad = ahrs.filters.TRIAD(w1=a, w2=m, as_quaternion=True)
>>> triad.A
array([[ 0.27531229, -0.00664771,  0.02275202,  0.96106259],
       [ 0.2793823 , -0.01030667,  0.0268131 ,  0.95975016],
       [ 0.28874411, -0.01551933,  0.03433374,  0.95666461]])

The first disadvantage is that TRIAD can only use two observations per estimation. If there are more observations, we must discard part of them (losing accuracy), or mix them in such a way that we obtain only two representative observations.

The second disadvantage is its loss of accuracy in a heavily dynamic state of the measuring device. TRIAD assumes a quasi-static state of the body frame and, therefore, its use is limited to motionless objects, preferably.

Footnotes

[1]This package’s author resides in Munich, and examples of geographical locations will take it as a reference.

References

[Black]Black, Harold. “A Passive System for Determining the Attitude of a Satellite,” AIAA Journal, Vol. 2, July 1964, pp. 1350–1351.
[Lerner1](1, 2) Lerner, G. M. “Three-Axis Attitude Determination” in Spacecraft Attitude Determination and Control, edited by J.R. Wertz. 1978. p. 420-426.
[Hall]Chris Hall. Spacecraft Attitude Dynamics and Control. Chapter 4: Attitude Determination. 2003. (http://www.dept.aoe.vt.edu/~cdhall/courses/aoe4140/attde.pdf)
[Makley]F.L. Makley et al. Fundamentals of Spacecraft Attitude Determination and Control. 2014. Pages 184-186.
[Shuster2007]Shuster, Malcolm D. The optimization of TRIAD. The Journal of the Astronautical Sciences, Vol. 55, No 2, April – June 2007, pp. 245–257. (http://www.malcolmdshuster.com/Pub_2007f_J_OptTRIAD_AAS.pdf)
class ahrs.filters.triad.TRIAD(w1: numpy.ndarray = None, w2: numpy.ndarray = None, v1: numpy.ndarray = None, v2: numpy.ndarray = None, representation: str = 'rotmat', frame: str = 'NED')

Tri-Axial Attitude Determination

TRIAD estimates the attitude as a Direction Cosine Matrix. To return it as a quaternion, set the parameter as_quaternion to True.

Parameters:
  • w1 (numpy.ndarray) – First tri-axial observation vector in body frame. Usually a normalized acceleration vector \(\mathbf{a} = \begin{bmatrix}a_x & a_y & a_z \end{bmatrix}\)
  • w2 (numpy.ndarray) – Second tri-axial observation vector in body frame. Usually a normalized magnetic field vector \(\mathbf{m} = \begin{bmatrix}m_x & m_y & m_z \end{bmatrix}\)
  • v1 (numpy.ndarray, optional.) – First tri-axial reference vector. Defaults to normalized gravity vector \(\mathbf{g} = \begin{bmatrix}0 & 0 & 1 \end{bmatrix}\)
  • v2 (numpy.ndarray, optional.) – Second tri-axial reference vector. Defaults to normalized geomagnetic field \(\mathbf{h} = \begin{bmatrix}h_x & h_y & h_z \end{bmatrix}\) in Munich, Germany.
  • representation (str, default: 'rotmat') – Attitude representation. Options are rotmat', 'quaternion', 'rpy', or 'axisangle'.
  • frame (str, default: 'NED') – Local tangent plane coordinate frame. Valid options are right-handed 'NED' for North-East-Down and 'ENU' for East-North-Up.
Variables:
  • w1 (numpy.ndarray) – First tri-axial observation vector in body frame.
  • w2 (numpy.ndarray) – Second tri-axial observation vector in body frame.
  • v1 (numpy.ndarray, optional.) – First tri-axial reference vector.
  • v2 (numpy.ndarray, optional.) – Second tri-axial reference vector.
  • as_quaternion (bool, default: False) – Whether to return attitude as a quaternion.
  • A (numpy.ndarray) – Estimated attitude.

Examples

>>> from ahrs.filters import TRIAD
>>> triad = TRIAD()
>>> triad.v1 = np.array([0.0, 0.0, 1.0])                    # Reference gravity vector (g)
>>> triad.v2 = np.array([21.0097, 1.3335, 43.732])          # Reference geomagnetic field (h)
>>> a = np.array([-2.499e-04, 4.739e-02, 0.9988763])        # Measured acceleration (normalized)
>>> a /= np.linalg.norm(a)
>>> m = np.array([-0.36663061, 0.17598138, -0.91357132])    # Measured magnetic field (normalized)
>>> m /= np.linalg.norm(m)
>>> triad.estimate(w1=a, w2=m)
array([[-8.48320410e-01, -5.29483162e-01, -2.49900033e-04],
       [ 5.28878238e-01, -8.47373587e-01,  4.73900062e-02],
       [-2.53039690e-02,  4.00697428e-02,  9.98876431e-01]])

It also works by passing each array to its corresponding parameter. They will be normalized too.

>>> triad = TRIAD(w1=a, w2=m, v1=[0.0, 0.0, 1.0], v2=[-0.36663061, 0.17598138, -0.91357132])
estimate(w1: numpy.ndarray, w2: numpy.ndarray, representation: str = 'rotmat') → numpy.ndarray

Attitude Estimation.

The equation numbers in the code refer to [Lerner1].

Parameters:
  • w1 (numpy.ndarray) – Sample of first tri-axial sensor.
  • w2 (numpy.ndarray) – Sample of second tri-axial sensor.
  • as_quaternion (bool, default: False) – Return estimated attitude as a quaternion representation.
Returns:

A – Estimated attitude as 3-by-3 Direction Cosine Matrix. If as_quaternion is set to True, it is returned as a quaternion representation.

Return type:

numpy.ndarray

Examples

>>> triad = ahrs.filters.TRIAD()
>>> triad.v1 = [0.0, 0.0, 1.0]                              # Normalized reference gravity vector (g)
>>> triad.v2 = [0.4328755, 0.02747412, 0.90103495]          # Normalized reference geomagnetic field (h)
>>> a = [4.098297, 8.663757, 2.1355896]                     # Measured acceleration
>>> m = [-28715.50512, -25927.43566, 4756.83931]            # Measured magnetic field
>>> triad.estimate(w1=a, w2=m)                              # Estimated attitude as DCM
array([[-7.84261e-01  ,  4.5905718e-01,  4.1737417e-01],
       [ 2.2883429e-01, -4.1126404e-01,  8.8232463e-01],
       [ 5.7668844e-01,  7.8748232e-01,  2.1749032e-01]])
>>> triad.estimate(w1=a, w2=m, as_quaternion=True)          # Estimated attitude as quaternion
array([ 0.07410345, -0.3199659, -0.53747247, -0.77669417])