Attitude from gravity (Tilt)#

Attitude estimation via gravity acceleration measurements.

The simplest way to estimate the attitude from the gravitational acceleration is using 3D geometric quadrants.

Although some methods use arctan to estimate the angles [ST-AN4509] [AD-AN1057], it is preferred to use arctan2 to explore all quadrants searching the tilt angles.

First, we normalize the gravity vector, so that it has magnitude equal to 1. Then, we get the angles to the main axes with arctan2 [FS-AN3461] [Trimpe]:

\[\begin{split}\begin{array}{ll} \theta &= \mathrm{arctan2}(a_y, a_z) \\ \phi &= \mathrm{arctan2}\big(-a_x, \sqrt{a_y^2+a_z^2}\big) \end{array}\end{split}\]

where \(\theta\) is the roll angle, \(\phi\) is the pitch angle, and \(\mathbf{a}=\begin{bmatrix}a_x & a_y & a_z\end{bmatrix}^T\) is the normalized vector of measured accelerations, which means \(\|\mathbf{a}\|=1\).

The attitude in terms of these two angles is called the tilt.

Heading angle

The heading angle, a.k.a. yaw, cannot be obtained from the measured acceleration, and a different reference shall be used to obtain it. The most common is the use of the geomagnetic information, in other words, Earth’s magnetic field.

With the pitch and roll angles estimated from the accelerometer, we can rotate a magnetometer reading \(\mathbf{m}=\begin{bmatrix}m_x & m_y & m_z\end{bmatrix}^T\), and estimate the yaw angle \(\psi\) to update the orientation.

The vector \(\mathbf{b}=\begin{bmatrix}b_x & b_y & b_z\end{bmatrix}^T\) represents the magnetometer readings after rotating them back to the plane, where \(\theta = \phi = 0\).

\[\begin{split}\begin{array}{cl} \mathbf{b} &= R_y(-\theta)R_x(-\phi)\mathbf{m} = R_y(\theta)^TR_x(\phi)^T\mathbf{m} \\ &= \begin{bmatrix} \cos\theta & \sin\theta\sin\phi & \sin\theta\cos\phi \\ 0 & \cos\phi & -\sin\phi \\ -\sin\theta & \cos\theta\sin\phi & \cos\theta\cos\phi \end{bmatrix} \begin{bmatrix}m_x \\ m_y \\ m_z\end{bmatrix} \\ \begin{bmatrix}b_x \\ b_y \\ b_z\end{bmatrix} &= \begin{bmatrix} m_x\cos\theta + m_y\sin\theta\sin\phi + m_z\sin\theta\cos\phi \\ m_y\cos\phi - m_z\sin\phi \\ -m_x\sin\theta + m_y\cos\theta\sin\phi + m_z\cos\theta\cos\phi \end{bmatrix} \end{array}\end{split}\]

Where \(\mathbf{m}=\begin{bmatrix}m_x & m_y & m_z\end{bmatrix}^T\) is the normalized vector of the measured magnetic field, which means \(\|\mathbf{m}\|=1\).

The yaw angle \(\psi\) is the tilt-compensated heading angle relative to magnetic North, computed as [FS-AN4248]:

\[\begin{split}\begin{array}{ll} \psi &= \mathrm{arctan2}(-b_y, b_x) \\ &= \mathrm{arctan2}\big(m_z\sin\phi - m_y\cos\phi, \; m_x\cos\theta + \sin\theta(m_y\sin\phi + m_z\cos\phi)\big) \end{array}\end{split}\]

Finally, we transform the roll-pitch-yaw angles to a quaternion representation:

\[\begin{split}\mathbf{q} = \begin{pmatrix}q_w\\q_x\\q_y\\q_z\end{pmatrix} = \begin{pmatrix} \cos\Big(\frac{\phi}{2}\Big)\cos\Big(\frac{\theta}{2}\Big)\cos\Big(\frac{\psi}{2}\Big) + \sin\Big(\frac{\phi}{2}\Big)\sin\Big(\frac{\theta}{2}\Big)\sin\Big(\frac{\psi}{2}\Big) \\ \sin\Big(\frac{\phi}{2}\Big)\cos\Big(\frac{\theta}{2}\Big)\cos\Big(\frac{\psi}{2}\Big) - \cos\Big(\frac{\phi}{2}\Big)\sin\Big(\frac{\theta}{2}\Big)\sin\Big(\frac{\psi}{2}\Big) \\ \cos\Big(\frac{\phi}{2}\Big)\sin\Big(\frac{\theta}{2}\Big)\cos\Big(\frac{\psi}{2}\Big) + \sin\Big(\frac{\phi}{2}\Big)\cos\Big(\frac{\theta}{2}\Big)\sin\Big(\frac{\psi}{2}\Big) \\ \cos\Big(\frac{\phi}{2}\Big)\cos\Big(\frac{\theta}{2}\Big)\sin\Big(\frac{\psi}{2}\Big) - \sin\Big(\frac{\phi}{2}\Big)\sin\Big(\frac{\theta}{2}\Big)\cos\Big(\frac{\psi}{2}\Big) \end{pmatrix}\end{split}\]

Setting the property as_angles to True will avoid this last conversion returning the attitude as angles.

References

[Trimpe]

Sebastian Trimpe and Raffaello D’Andrea. The Balancing cube. A dynamic sculpture as test bed for distributed estimation and control. IEEE Control Systems Magazine. December 2012. (http://trimpe.is.tuebingen.mpg.de/publications/trimpe-CSM12.pdf)

[FS-AN3461]

Mark Pedley. Tilt Sensing Using a Three-Axis Accelerometer. Freescale Semiconductor Application Note. Document Number: AN3461. 2013. (https://www.nxp.com/files-static/sensors/doc/app_note/AN3461.pdf)

[FS-AN4248]

Talat Ozyagcilar. Implementing a Tilt-Compensated eCompass using Accelerometer and Magnetometer sensors. Freescale Semoconductor Application Note. Document Number: AN4248. 2015. (https://www.nxp.com/files-static/sensors/doc/app_note/AN4248.pdf)

[AD-AN1057]

Christopher J. Fisher. Using an Accelerometer for Inclination Sensing. Analog Devices. Application Note. AN-1057. (https://www.analog.com/media/en/technical-documentation/application-notes/AN-1057.pdf)

[ST-AN4509]

Tilt measurement using a low-g 3-axis accelerometer. STMicroelectronics. Application note AN4509. 2014. (https://www.st.com/resource/en/application_note/dm00119046.pdf)

[WikiConversions]

Wikipedia: Conversion between quaternions and Euler angles. (https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles)

class ahrs.filters.tilt.Tilt(acc: ndarray | None = None, mag: ndarray | None = None, **kwargs)#

Gravity-based estimation of attitude.

Parameters:

  • acc (numpy.ndarray, default: None) – N-by-3 array with measurements of gravitational acceleration.

  • mag (numpy.ndarray, default: None) – N-by-3 array with measurements of local geomagnetic field.

  • representation (str, default: 'quaternion') – Attitude representation. Options are 'quaternion', 'angles' or 'rotmat'.

Variables:

  • acc (numpy.ndarray) – N-by-3 array with N tri-axial accelerometer samples.

  • mag (numpy.ndarray) – N-by-3 array with N tri-axial magnetometer samples.

  • Q (numpy.ndarray, default: None) – N-by-4 or N-by-3 array with N quaternions.

Raises:

ValueError – When shape of input arrays are not (3,) or (N, 3).

Examples

Assuming we have 3-axis accelerometer data in N-by-3 arrays, we can simply give these samples to the constructor. The tilt estimation works solely with accelerometer samples.

>>> from ahrs.filters import Tilt
>>> tilt = Tilt(acc_data)

The estimated quaternions are saved in the attribute Q.

>>> tilt.Q
array([[0.76901856, 0.60247641, -0.16815772, 0.13174072],
       [0.77310283, 0.59724644, -0.16900433, 0.1305612 ],
       [0.7735134,  0.59644005, -0.1697294,  0.1308748 ],
       ...,
       [0.7800751,  0.59908629, -0.14315079, 0.10993772],
       [0.77916118, 0.59945374, -0.14520157, 0.11171197],
       [0.77038613, 0.61061868, -0.14375869, 0.11394512]])
>>> tilt.Q.shape
(1000, 4)

If we desire to estimate each sample independently, we call the corresponding method with each sample individually.

>>> tilt = Tilt()
>>> num_samples = len(acc_data)
>>> Q = np.zeros((num_samples, 4))  # Allocate quaternions array
>>> for t in range(num_samples):
...     Q[t] = tilt.estimate(acc_data[t])
...
>>> tilt.Q[:5]
array([[0.76901856, 0.60247641, -0.16815772, 0.13174072],
       [0.77310283, 0.59724644, -0.16900433, 0.1305612 ],
       [0.7735134,  0.59644005, -0.1697294,  0.1308748 ],
       [0.77294791, 0.59913005, -0.16502363, 0.12791369],
       [0.76936935, 0.60323746, -0.16540014, 0.12968487]])

Originally, this estimation computes first the Roll-Pitch-Yaw angles and then converts them to Quaternions. If we desire the angles instead, we set it so in the parameters.

>>> tilt = Tilt(acc_data, representation='angles')
>>> type(tilt.Q), tilt.Q.shape
(<class 'numpy.ndarray'>, (1000, 3))
>>> tilt.Q[:5]
array([[8.27467200e-04,  4.36167791e-06, 0.00000000e+00],
       [9.99352822e-04,  8.38015258e-05, 0.00000000e+00],
       [1.30423484e-03,  1.72201573e-04, 0.00000000e+00],
       [1.60337482e-03,  8.53081042e-05, 0.00000000e+00],
       [1.98459171e-03, -8.34729603e-05, 0.00000000e+00]])

Note

It will return the angles, in degrees, following the standard order Roll->Pitch->Yaw.

The yaw angle is, expectedly, equal to zero, because the heading cannot be estimated with the gravity acceleration only.

For this reason, magnetometer data can be used to estimate the yaw. This is also implemented and the magnetometer will be taken into account when given as parameter.

>>> tilt = Tilt(acc=acc_data, mag=mag_data, representation='angles')
>>> tilt.Q[:5]
array([[8.27467200e-04,  4.36167791e-06, -4.54352439e-02],
       [9.99352822e-04,  8.38015258e-05, -4.52836926e-02],
       [1.30423484e-03,  1.72201573e-04, -4.49355365e-02],
       [1.60337482e-03,  8.53081042e-05, -4.44276770e-02],
       [1.98459171e-03, -8.34729603e-05, -4.36931634e-02]])
estimate(acc: ndarray, mag: ndarray | None = None, representation: str = 'quaternion') → ndarray#

Estimate the quaternion from the tilting read by an orthogonal tri-axial array of accelerometers.

The orientation of the roll and pitch angles is estimated using the measurements of the accelerometers, and finally converted to a quaternion representation according to [WikiConversions]

Parameters:

  • acc (numpy.ndarray) – Sample of tri-axial Accelerometer in m/s^2.

  • mag (numpy.ndarray, default: None) – N-by-3 array with measurements of magnetic field in mT.

Returns:

q – Estimated attitude.

Return type:

numpy.ndarray

Examples

>>> acc_data = np.array([4.098297, 8.663757, 2.1355896])
>>> mag_data = np.array([-28.71550512, -25.92743566, 4.75683931])
>>> from ahrs.filters import Tilt
>>> tilt = Tilt()
>>> tilt.estimate(acc=acc_data, mag=mag_data)   # Estimate attitude as quaternion
array([0.09867706 0.33683592 0.52706394 0.77395607])

Optionally, the attitude can be retrieved as roll-pitch-yaw angles, in degrees.

>>> tilt = Tilt(as_angles=True)
>>> tilt.estimate(acc=acc_data, mag=mag_data)
array([ 76.15281566 -24.66891862 146.02634429])