Fast Linear Attitude Estimator#

The Fast Linear Attitude Estimator (FLAE) obtains the attitude quaternion with an eigenvalue-based solution as proposed by [Wu].

A symbolic solution to the corresponding characteristic polynomial is also derived for a higher computation speed.

One-Dimensional Fusion#

We assume that we have a single observable (can be measured) frame. The sensor outputs can be rotated with a \(3\times 3\) Direction Cosine Matrix \(\mathbf{C}\) using:

\[\mathbf{D}^b = \mathbf{CD}^r\]

where \(\mathbf{D}^b=\begin{bmatrix}D_x^b & D_y^b & D_z^b\end{bmatrix}^T\) is the observation vector in body frame and \(\mathbf{D}^r=\begin{bmatrix}D_x^r & D_y^r & D_z^r\end{bmatrix}^T\) is the observation vector in reference frame. To put it in terms of a quaternion, we define the loss function \(\mathbf{f}_D(\mathbf{q})\) as:

\[\mathbf{f}_D(\mathbf{q}) \triangleq \mathbf{CD}^r - \mathbf{D}^b\]

where the quaternion \(\mathbf{q}\) is defined as:

\[\begin{split}\begin{array}{rcl} \mathbf{q}&=&\begin{pmatrix}q_w & q_x & q_y & q_z\end{pmatrix}^T \\ &=& \begin{pmatrix}\cos\frac{\theta}{2} & n_x\sin\frac{\theta}{2} & n_y\sin\frac{\theta}{2} & n_z\sin\frac{\theta}{2}\end{pmatrix}^T \end{array}\end{split}\]

The purpose is to minimize the loss function. We start by expanding \(\mathbf{f}_D(\mathbf{q})\):

\[\begin{split}\begin{array}{rcl} \mathbf{f}_D(\mathbf{q}) &=& \mathbf{CD}^r - \mathbf{D}^b \\ &=& \mathbf{P}_D\mathbf{q} - \mathbf{D}^b \\ &=& (D_x^r\mathbf{P}_1 + D_y^r\mathbf{P}_2 + D_z^r\mathbf{P}_3)\mathbf{q} - \mathbf{D}^b \\ &=& D_x^r\mathbf{C}_1 + D_y^r\mathbf{C}_2 + D_z^r\mathbf{C}_3 - \mathbf{D}^b \end{array}\end{split}\]

where \(\mathbf{C}_1\), \(\mathbf{C}_2\) and \(\mathbf{C}_3\) are the columns of \(\mathbf{C}\) represented as:

\[\begin{split}\begin{array}{rcl} \mathbf{C}_1 &=& \mathbf{P}_1\mathbf{q} = \begin{bmatrix}q_w^2+q_x^2-q_y^2-q_z^2 \\ 2(q_xq_y + q_wq_z) \\ 2(q_xq_z - q_wq_y) \end{bmatrix} \\ && \\ \mathbf{C}_2 &=& \mathbf{P}_2\mathbf{q} = \begin{bmatrix}2(q_xq_y - q_wq_z) \\ q_w^2-q_x^2+q_y^2-q_z^2 \\2(q_wq_x + q_yq_z) \end{bmatrix} \\ && \\ \mathbf{C}_3 &=& \mathbf{P}_3\mathbf{q} = \begin{bmatrix}2(q_xq_z + q_wq_y) \\ 2(q_yq_z - q_wq_x) \\ q_w^2-q_x^2-q_y^2+q_z^2 \end{bmatrix} \end{array}\end{split}\]

When \(\mathbf{q}\) is optimal, it satisfies:

\[\mathbf{q} = \mathbf{P}_D^\dagger \mathbf{D}^b\]

where \(\mathbf{P}_D^\dagger\) is the pseudo-inverse of \(\mathbf{P}_D\) if and only if it has full rank.

Note

A matrix is said to have full rank if its rank is equal to the largest possible for a matrix of the same dimensions, which is the lesser of the number of rows and columns.

The analytical form of any pseudo-inverse is normally difficult to obtain, but thanks to the orthogonality of \(\mathbf{P}_D\) we get:

\[\mathbf{P}_D^\dagger = \mathbf{P}_D^T = D_x^r\mathbf{P}_1^T + D_y^r\mathbf{P}_2^T + D_z^r\mathbf{P}_3^T\]

The orientation \(\mathbf{q}\) is obtained from:

\[\mathbf{P}_D^\dagger\mathbf{D}^b - \mathbf{q} = \mathbf{Gq}\]

Solving \(\mathbf{Gq}=0\) (if and only if \(\mathrm{det}(\mathbf{G})=0\)) using elementary row transformations we obtain the wanted orthonormal quaternion.

N-Dimensional Fusion#

We assume having \(n\) observation equations, such that the error residual vector is given by augmenting \(\mathbf{f}_D(\mathbf{q})\) as:

\[\begin{split}\mathbf{f}_{\Sigma D}(\mathbf{q}) = \begin{bmatrix} \sqrt{a_1}(\mathbf{P}_{D_1}\mathbf{q}-D_1^b) \\ \sqrt{a_2}(\mathbf{P}_{D_2}\mathbf{q}-D_2^b) \\ \vdots \\ \sqrt{a_n}(\mathbf{P}_{D_n}\mathbf{q}-D_n^b) \end{bmatrix}\end{split}\]

When \(\mathbf{f}_{\Sigma D}(\mathbf{q})=0\), the equation satisfies:

\[\begin{split}\begin{array}{rcl} \mathbf{P}_{\Sigma D}\mathbf{q} &=& \mathbf{D}_\Sigma^b \\ \begin{bmatrix} \sqrt{a_1}\mathbf{P}_{D_1} \\ \sqrt{a_2}\mathbf{P}_{D_2} \\ \vdots \\ \sqrt{a_n}\mathbf{P}_{D_n} \end{bmatrix}\mathbf{q} &=& \begin{bmatrix} \sqrt{a_1}\mathbf{D}_1^b \\ \sqrt{a_2}\mathbf{D}_2^b \\ \vdots \\ \sqrt{a_n}\mathbf{D}_n^b \end{bmatrix} \end{array}\end{split}\]

Intuitively, we would solve it with \(\mathbf{q}=\mathbf{P}_{\Sigma D}^\dagger\mathbf{D}_\Sigma^b\), but the pseudo-inverse of \(\mathbf{P}_{\Sigma D}\) is very difficult to compute. However, it is possible to transform the equation by the pseudo-inverse matrices of \(\mathbf{q}\) and \(\mathbf{D}_\Sigma^b\):

\[\mathbf{q}^\dagger = (\mathbf{D}_\Sigma^b)^\dagger \mathbf{P}_{\Sigma D}\]

\(\mathbf{P}_{\Sigma D}\) can be further expanded into:

\[\mathbf{P}_{\Sigma D} = \mathbf{U}_{D_x}\mathbf{P}_1 + \mathbf{U}_{D_y}\mathbf{P}_2 + \mathbf{U}_{D_z}\mathbf{P}_3\]

where \(\mathbf{P}_1\), \(\mathbf{P}_2\) and \(\mathbf{P}_3\) are \(3\times 4\) matrices, and \(\mathbf{U}_{D_x}\), \(\mathbf{U}_{D_y}\) and \(\mathbf{U}_{D_z}\) are \(3n\times 3\) matrices. Hence,

\[(\mathbf{D}_\Sigma^b)^\dagger\mathbf{P}_{\Sigma D} = (\mathbf{D}_\Sigma^b)^\dagger\mathbf{U}_{D_x}\mathbf{P}_1 + (\mathbf{D}_\Sigma^b)^\dagger\mathbf{U}_{D_y}\mathbf{P}_2 + (\mathbf{D}_\Sigma^b)^\dagger\mathbf{U}_{D_z}\mathbf{P}_3\]

The fusion equation finally arrives to:

\[\mathbf{H}_x\mathbf{P}_1 + \mathbf{H}_y\mathbf{P}_2 + \mathbf{H}_z\mathbf{P}_3 - \mathbf{q}^\dagger = \mathbf{0}_{1\times 4}\]

where \(\mathbf{H}_x\), \(\mathbf{H}_y\) and \(\mathbf{H}_z\) are \(1\times 3\) matrices

\[\begin{split}\begin{array}{rcl} \mathbf{H}_x &= (\mathbf{D}_\Sigma^b)^\dagger\mathbf{U}_{D_x} =& \begin{bmatrix} \sum_{i=1}^n a_iD_{x,i}^rD_{x,i}^b & \sum_{i=1}^n a_iD_{x,i}^rD_{y,i}^b & \sum_{i=1}^n a_iD_{x,i}^rD_{z,i}^b & \end{bmatrix} \\ && \\ \mathbf{H}_y &= (\mathbf{D}_\Sigma^b)^\dagger\mathbf{U}_{D_y} =& \begin{bmatrix} \sum_{i=1}^n a_iD_{y,i}^rD_{x,i}^b & \sum_{i=1}^n a_iD_{y,i}^rD_{y,i}^b & \sum_{i=1}^n a_iD_{y,i}^rD_{z,i}^b & \end{bmatrix} \\ && \\ \mathbf{H}_z &= (\mathbf{D}_\Sigma^b)^\dagger\mathbf{U}_{D_z} =& \begin{bmatrix} \sum_{i=1}^n a_iD_{z,i}^rD_{x,i}^b & \sum_{i=1}^n a_iD_{z,i}^rD_{y,i}^b & \sum_{i=1}^n a_iD_{z,i}^rD_{z,i}^b & \end{bmatrix} \end{array}\end{split}\]

Refactoring the equation with a transpose operation, we obtain:

\[\begin{split}\begin{array}{rcl} \mathbf{P}_1^T\mathbf{H}_x^T + \mathbf{P}_2^T\mathbf{H}_y^T + \mathbf{P}_3^T\mathbf{H}_z^T - \mathbf{q} &=& \mathbf{0} \\ (\mathbf{W} - \mathbf{I})\mathbf{q} &=& \mathbf{0} \end{array}\end{split}\]

where the elements of \(\mathbf{W}\) are given by:

\[\begin{split}\mathbf{W} = \begin{bmatrix} H_{x1} + H_{y2} + H_{z3} & -H_{y3} + H_{z2} & -H_{z1} + H_{x3} & -H_{x2} + H_{y1} \\ -H_{y3} + H_{z2} & H_{x1} - H_{y2} - H_{z3} & H_{x2} + H_{y1} & H_{x3} + H_{z1} \\ -H_{z1} + H_{x3} & H_{x2} + H_{y1} & H_{y2} - H_{x1} - H_{z3} & H_{y3} + H_{z2} \\ -H_{x2} + H_{y1} & H_{x3} + H_{z1} & H_{y3} + H_{x2} & H_{z3} - H_{y2} - H_{x1} \end{bmatrix}\end{split}\]

Eigenvector solution#

The simplest solution is to find the eigenvector corresponding to the largest eigenvalue of \(\mathbf{W}\), as used by Davenport’s q-method.

This has the advantage of returning a normalized and valid quaternion, which is used to represent the attitude.

This method’s main disadvantage is \((\mathbf{W}-\mathbf{I})\) suffering from rank-deficient problems in the sensor outputs, besides its computational cost.

Characteristic Polynomial#

The fusion equation can be transformed by adding a small quaternion error \(\epsilon \mathbf{q}\)

\[\mathbf{Wq} = (1+\epsilon)\mathbf{q}\]

recognizing that \(1+\epsilon\) is an eigenvalue of \(\mathbf{W}\) the problem is now shifted to find the eigenvalue that is closest to 1.

Analytically the calculation of the eigenvalue of \(\mathbf{W}\) builds first its characteristic polynomial as:

\[\begin{split}\begin{array}{rcl} f(\lambda) &=& \mathrm{det}(\mathbf{W}-\lambda\mathbf{I}_{4\times 4}) \\ &=& \lambda^4 + \tau_1\lambda^2 + \tau_2\lambda + \tau_3 \end{array}\end{split}\]

where the coefficients are obtained from:

\[\begin{split}\begin{array}{rcl} \tau_1 &=& -2\big(H_{x1}^2 + H_{x2}^2 + H_{x3}^2 + H_{y1}^2 + H_{y2}^2 + H_{y3}^2 + H_{z1}^2 + H_{z2}^2 + H_{z3}^2\big) \\ && \\ \tau_2 &=& 8\big(H_{x3}H_{y2}H_{z1} - H_{x2}H_{y3}H_{z1} - H_{x3}H_{y1}H_{z2} + H_{x1}H_{y3}H_{z2} + H_{x2}H_{y1}H_{z3} - H_{x1}H_{y2}H_{z3}\big) \\ && \\ \tau_3 &=& \mathrm{det}(\mathbf{W}) \end{array}\end{split}\]

Once \(\lambda\) is defined, the eigenvector can be obtained using elementary row operations (Gaussian elimination).

There are two main methods to compute the optimal \(\lambda\):

1. Iterative Newton-Raphson method

This 4th-order characteristic polynomial \(f(\lambda)\) can be solved with the Newton-Raphson’s method and the aid of its derivative, which is found to be:

\[f'(\lambda) = 4\lambda^3 + 2\tau_1\lambda + \tau_2\]

The initial value for the root finding process can be set to 1, because \(\lambda\) is very close to it. So, every iteration at \(n\) updates \(\lambda\) as:

\[\lambda_{n+1} \gets \lambda_n - \frac{f\big(\lambda_n\big)}{f'\big(\lambda_n\big)} = \lambda_n - \frac{\lambda_n^4 + \tau_1\lambda_n^2 + \tau_2\lambda_n + \tau_3}{4\lambda_n^3 + 2\tau_1\lambda_n + \tau_2}\]

The value of \(\lambda\) is commonly found after a couple iterations, but the accuracy is not linear with the iteration steps and will not always achieve good results.

2. Symbolic method

A more precise solution involves a symbolic approach, where four solutions to the characteristic polynomial are obtained as follows:

\[\begin{split}\begin{array}{rcl} \lambda_1 &=& \alpha \Big(T_2 - \sqrt{k_1 - k_2}\Big) \\ && \\ \lambda_2 &=& \alpha \Big(T_2 + \sqrt{k_1 - k_2}\Big) \\ && \\ \lambda_3 &=& -\alpha \Big(T_2 + \sqrt{k_1 + k_2}\Big) \\ && \\ \lambda_4 &=& -\alpha \Big(T_2 - \sqrt{k_1 + k_2}\Big) \\ \end{array}\end{split}\]

with the helper variables:

\[\begin{split}\begin{array}{rcl} \alpha &=& \frac{1}{2\sqrt{6}} \\ && \\ k_1 &=& -T_2^2-12\tau_1 \\ && \\ k_2 &=& \frac{12\sqrt{6}\tau_2}{T_2} \\ && \\ T_0 &=& 2\tau_1^3 + 27\tau_2^2 - 72\tau_1\tau_3 \\ && \\ T_1 &=& \Big(T_0 + \sqrt{-4(t_1^2+12\tau_3)^3 + T_0^2}\Big)^{\frac{1}{3}} \\ && \\ T_2 &=& \sqrt{-4\tau_1 + \frac{2^{\frac{4}{3}}(\tau_1^2+12\tau_3)}{T_1} + 2^{\frac{2}{3}}T_1} \end{array}\end{split}\]

Then chose the \(\lambda\), which is closest to 1. This way solving for \(\lambda\) is truly shortened.

Optimal Quaternion#

Having \(\mathbf{N}=\mathbf{W}-\lambda\mathbf{I}_{4\times 4}\), the matrix can be transformed via row operations to:

\[\begin{split}\mathbf{N} \to \mathbf{N}' = \begin{bmatrix} 1 & 0 & 0 & \chi \\ 0 & 1 & 0 & \rho \\ 0 & 0 & 1 & \upsilon \\ 0 & 0 & 0 & \zeta \end{bmatrix}\end{split}\]

where \(\zeta\) is usually a very small number. To ensure that \((\mathbf{W}-\lambda\mathbf{I}_{4\times 4})=\mathbf{q}=\mathbf{0}\) has non-zero and unique solution, \(\zeta\) is chosen to be 0. Hence:

\[\begin{split}\mathbf{N}' = \begin{bmatrix} 1 & 0 & 0 & \chi \\ 0 & 1 & 0 & \rho \\ 0 & 0 & 1 & \upsilon \\ 0 & 0 & 0 & 0 \end{bmatrix}\end{split}\]

Letting \(q_w=-1\), the solution to the optimal quaternion is obtained with:

\[\begin{split}\mathbf{q} = \begin{pmatrix}q_w\\q_x\\q_y\\q_z\end{pmatrix} = \begin{pmatrix}-1\\\chi\\\rho\\\upsilon\end{pmatrix}\end{split}\]

Finally, the quaternion is normalized to be able to be used as a versor:

\[\mathbf{q} = \frac{1}{\|\mathbf{q}\|} \mathbf{q}\]

The decisive element of QUEST is its matrix \(\mathbf{K}\), whereas for FLAE \(\mathbf{W}\) plays the same essential role. Both algorithms spend most of its computation obtaining said matrices.

FLAE has the same accuracy as other similar estimators (QUEST, SVD, etc.), but with the advantage of being up to 47% faster than the fastest among them.

Another advantage is the symbolic formulation of the characteristic polynomial, which does not contain any adjoint matrices, leading to a simpler (therefore faster) calculation of the eigenvalues.

FLAE advocates for the symbolic method to calculate the eigenvalue. However, the Newton iteration can be also used to achieve a similar performance to that of QUEST.

References

[Wu]

Jin Wu, Zebo Zhou, Bin Gao, Rui Li, Yuhua Cheng, et al. Fast Linear Quaternion Attitude Estimator Using Vector Observations. IEEE Transactions on Automation Science and Engineering, Institute of Electrical and Electronics Engineers, 2018. (https://hal.inria.fr/hal-01513263)

class ahrs.filters.flae.FLAE(acc: ndarray | None = None, mag: ndarray | None = None, method: str = 'symbolic', **kw)#

Fast Linear Attitude Estimator

Parameters:

  • acc (numpy.ndarray, default: None) – N-by-3 array with measurements of acceleration in in m/s^2

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

  • method (str, default: 'symbolic') – Method used to estimate the attitude. Options are: 'symbolic', 'eig', and 'newton'.

  • weights (np.ndarray, default: [0.5, 0.5]) – Weights used for each sensor. They must add up to 1.

  • magnetic_dip (float) – Geomagnetic Inclination angle at local position, in degrees. Defaults to magnetic dip of Munich, Germany.

Raises:

ValueError – When estimation method is invalid.

Examples

>>> orientation = FLAE()
>>> accelerometer = np.array([-0.2853546, 9.657394, 2.0018768])
>>> magnetometer = np.array([12.32605, -28.825378, -26.586914])
>>> orientation.estimate(acc=accelerometer, mag=magnetometer)
array([-0.45447247, -0.69524546,  0.55014011, -0.08622285])

You can set a different estimation method passing its name to parameter method.

>>> orientation.estimate(acc=accelerometer, mag=magnetometer, method='newton')
array([ 0.42455176,  0.68971918, -0.58315259, -0.06305803])

Or estimate all quaternions at once by giving the data to the constructor. All estimated quaternions are stored in attribute Q.

>>> orientation = FLAE(acc=acc_data, mag=mag_data, method='eig')
>>> orientation.Q.shape
(1000, 4)
estimate(acc: ndarray, mag: ndarray, method: str = 'symbolic') → ndarray#

Estimate a quaternion with the given measurements and weights.

Parameters:

  • acc (numpy.ndarray) – Sample of tri-axial Accelerometer.

  • mag (numpy.ndarray) – Sample of tri-axial Magnetometer.

  • method (str, default: 'symbolic') – Method used to estimate the attitude. Options are: 'symbolic', 'eig' and 'newton'.

Returns:

q – Estimated orienation as quaternion.

Return type:

numpy.ndarray

Examples

>>> accelerometer = np.array([-0.2853546, 9.657394, 2.0018768])
>>> magnetometer = np.array([12.32605, -28.825378, -26.586914])
>>> orientation = FLAE()
>>> orientation.estimate(acc=accelerometer, mag=magnetometer)
array([-0.45447247, -0.69524546,  0.55014011, -0.08622285])
>>> orientation.estimate(acc=accelerometer, mag=magnetometer, method='eig')
array([ 0.42455176,  0.68971918, -0.58315259, -0.06305803])
>>> orientation.estimate(acc=accelerometer, mag=magnetometer, method='newton')
array([ 0.42455176,  0.68971918, -0.58315259, -0.06305803])