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 [STM14]
[Fis10] 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 [Ped13] [TDAndrea12]:
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\).
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 [Ozy15]:
Finally, we transform the roll-pitch-yaw angles to a quaternion representation:
Setting the property as_angles
to True
will avoid this last conversion
returning the attitude as angles.
- class ahrs.filters.tilt.Tilt(acc: ndarray = None, mag: ndarray = 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, 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 [Wika].
- 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])