Fast Accelerometer-Magnetometer Combination¶
The most typical low-cost sensor system is the Accelerometer-Magnetometer combination (AMC). It integrates the local gravity and the Earth’s magnetic field together, forming a full-attitude estimation system.
The most famous attitude determination formulation is Wahba’s Problem, which sprung several solutions using Euler Angles, Direction Cosine Matrices and Quaternions.
The matrix operations in these solutions are the main focus of attention in this method. The operations are analytically simplified, where the accuracy is maintained, while the time consumption is reduced, yielding the Fast Accelerometer-Magnetometer Combination (FAMC), whose main contributions are:
- Analytic eigenvalue results are given for the dynamic magnetometer reference vector.
- Solution as quaternion representation from a simplification of Davenport’s q-method.
- Advantageous on time consumption, compared with existing solutions to Wahba’s problem.
The AMC relates a Direction Cosine Matrix, \(\mathbf{C}\), such that:
where \(^b\mathbf{a}=\begin{bmatrix}a_x&a_y&a_z\end{bmatrix}^T\) and \(^b\mathbf{m}=\begin{bmatrix}m_x&m_y&m_z\end{bmatrix}^T\) are normalized observation vectors from the tri-axial accelerometer and magnetometer sensors in the body frame \(b\), respectively.
The reference frame, \(r\), is chosen to follow the North-East-Down (NED) coordinate system.
Thus, the reference vectors are given by \(^r\mathbf{a}=\begin{bmatrix}0 & 0 & 1\end{bmatrix}^T\) and \(^r\mathbf{m}=\begin{bmatrix}m_N & m_E & m_D\end{bmatrix}^T\), although this solution neglects the Easterly geomagnetic field describing it based on the local magnetic dip \(\theta\) to form a simpler expression \(^r\mathbf{m}=\begin{bmatrix}\cos\theta & 0 & -\sin\theta\end{bmatrix}^T\)
The most common solution to Wahba’s problem relates two vector observation pairs with weights. Here, two equivalent weights equal to \(0.5\) yield the loss function:
Using Davenport’s q-method we find the minimum of \(L(\mathbf{C})\) by calculating the maximum eigenvalue of Davenport’s matrix \(\mathbf{K}\) in terms of quaternions:
Warning
The definition of the quaternion does NOT follow the common practice of setting the scalar part first, but actually putting the vector part first: \(\mathbf{q}=\begin{pmatrix}q_x & q_y & q_z & q_w\end{pmatrix}\). Consider this for all operations. The resulting attitude is, however, represented following the common definition at the final step: \(\mathbf{q}=\begin{pmatrix}q_w & q_x & q_y & q_z\end{pmatrix}\).
The helping arrays are:
Note
Indexing is normally starting from zero, especially in computational setups, but the article starts it from one, and it is kept like that in this documentation to coincide with the original document.
After knowing that \(\|\mathbf{a}\|=\|\mathbf{m}\|=m_N^2+m_D^2=1\), the four eigenvalues of \(\mathbf{K}\) are computed by:
The attitude quaternion is the eigenvector of \(\mathbf{K}\) corresponding to the maximum eigenvalue 1. This shows that the AMC is actually self-constrained and does not require the outer information of the magnetic dip angle.
Warning
When the AMC is used near the poles there may be two ambiguous solutions corresponding to an eigenvalue equal to 1, disorienting the system and rendering this estimator useless on polar regions.
Finally, we must compute the eigenvector of the eigenvalule 1. We start defining:
where \(\mathbf{S}\) can be further expanded with matrix row operations in echelon form \(\mathbf{T}=\mathbf{\Lambda}_1\mathbf{\Lambda}_2\mathbf{\Lambda}_3\mathbf{S}\):
So, \(\mathbf{T}\) is expanded as:
where:
The unnormalized scalar-wise quaternion is given by the solution:
which is easily normalized:
References
| [Liu] | Zhuohua Liu, Wei Liu, Xiangyang Gong, and Jin Wu, “Simplified Attitude Determination Algorithm Using Accelerometer and Magnetometer with Extremely Low Execution Time,” Journal of Sensors, vol. 2018, Article ID 8787236, 11 pages, 2018. https://doi.org/10.1155/2018/8787236. |
-
class
ahrs.filters.famc.FAMC(acc: numpy.ndarray = None, mag: numpy.ndarray = None)¶ Fast Accelerometer-Magnetometer Combination
Parameters: - acc (numpy.ndarray, default: None) – M-by-3 array with measurements of acceleration in in m/s^2
- mag (numpy.ndarray, default: None) – M-by-3 array with measurements of magnetic field in mT
Variables: - acc (numpy.ndarray) – M-by-3 array with M accelerometer samples.
- mag (numpy.ndarray) – M-by-3 array with m magnetometer samples.
- Q (numpy.array, default: None) – M-by-4 Array with all estimated quaternions, where M is the number of samples. Equal to None when no estimation is performed.
Raises: ValueError– When dimension of input arraysaccandmagare not equal.Examples
>>> acc_data.shape, mag_data.shape # NumPy arrays with sensor data ((1000, 3), (1000, 3)) >>> from ahrs.filters import FAMC >>> famc = FAMC(acc=acc_data, mag=mag_data) >>> famc.Q # Estimated attitudes as Quaternions array([[-0.82311077, 0.45760535, -0.33408929, -0.0383452 ], [-0.82522048, 0.4547043 , -0.33277675, -0.03892033], [-0.82463698, 0.4546915 , -0.33422422, -0.03903417], ..., [-0.82420642, 0.56217735, 0.02548005, -0.06317571], [-0.82364606, 0.56311099, 0.0241655 , -0.06268338], [-0.81844766, 0.57077781, 0.02532182, -0.06095017]]) >>> famc.Q.shape (1000, 4)
-
estimate(acc: numpy.ndarray, mag: numpy.ndarray) → numpy.ndarray¶ Attitude Estimation
Parameters: - a (numpy.ndarray) – Sample of tri-axial Accelerometer.
- m (numpy.ndarray) – Sample of tri-axial Magnetometer.
Returns: q – Estimated quaternion of the form \(\begin{pmatrix}q_w & q_x & q_y & q_z\end{pmatrix}\)
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 FAMC >>> famc = FAMC() >>> famc.estimate(acc=acc_data, mag=mag_data) # Estimate attitude as quaternion array([-0.82311077, 0.45760535, -0.33408929, -0.0383452])