Itzhack’s Method

ahrs.common.orientation.itzhack(dcm: ndarray, version: int = 3) → ndarray

Quaternion from a Direction Cosine Matrix with Bar-Itzhack’s method [BI00].

This method to compute the quaternion from a Direction Cosine Matrix (DCM) is based on the eigenvalue decomposition of the matrix \(\mathbf{K}\), and does not require any voting scheme like other known methods.

Moreover, this method is able to handle non-orthogonal matrices, while other methods require an orthogonal matrix to be used.

As defined in Wahba’s problem, we are looking for the quaternion, \(\mathbf{q} = [q_x, q_y, q_z, q_w]\), that minimizes the following cost function:

\[L(\mathbf{D}) = \frac{1}{2}\sum_{i=1}^{k}a_i|\mathbf{b}_i - \mathbf{D}\mathbf{r}_i|^2\]

where \(\mathbf{D}\) is the DCM obtained from the quaternion mathbf{q}:

\[\begin{split}\mathbf{D}(\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) \\ 2(q_xq_y + q_wq_z) & q_w^2 - q_x^2 + q_y^2 - q_z^2 & 2(q_yq_z - q_wq_x) \\ 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{split}\]

Warning

This method defines the quaternion as \([q_x, q_y, q_z, q_w]\) with a trailing scalar part, while other methods use \([q_w, q_x, q_y, q_z]\), with a leading scalar part.

The algebra in this documentation will be using Itzhack’s convention. To cope with this, this package’s implementation re-orders the quaternion at the end to match the most common definition.

\(\mathbf{r}\) are unit vectors in the reference coordinate frame, \(\mathbf{b}\) are the same vectors but in the body coordinate frame, and \(\mathbf{a}\) are a set of nonnegative weights assign to each pair.

Paul Davenport [Dav68] finds the optimal quaternion, \(\mathbf{q}^*\), that minimizes the cost function, through the eigenvalue decomposition of the matrix \(\mathbf{K}\):

\[\begin{split}\mathbf{K} = \begin{bmatrix} \mathbf{S} - \boldsymbol{\sigma}\mathbf{I}_3 & \mathbf{z} \\ \mathbf{z}^T & \boldsymbol{\sigma} \end{bmatrix}\end{split}\]

Davenport’s Method yields the quaternion describing a rotation from one frame to another when the components of at least two vectors in each frame are known in both frames.

If we know the precise DCM that characterizes a certain rotation, we can use it to generate such pairs, and then apply Daveport’s method back to these pairs, which yield a quaternion. Thus, we have computed the sought quaternion.

Itzhack’s algorithm has three versions depending on the given DCM. The first two algorithms are for a given orthogonal attitude matrix.

Version 1

Because only two vectors are necessary to determine attitude, we can simplify the computation choosing two unit vectors of the reference coordinates:

\[\begin{split}\begin{array}{rcl} \mathbf{r}_1^T &=& \begin{bmatrix} 1 & 0 & 0 \end{bmatrix} \\ \mathbf{r}_2^T &=& \begin{bmatrix} 0 & 1 & 0 \end{bmatrix} \end{array}\end{split}\]

From the relation \(\mathbf{b}_i = \mathbf{D}\mathbf{r}_i\), it is evident that the vectors in the body system that correspond to \(\mathbf{r}_1\) and \(\mathbf{r}_2\) are \(\mathbf{b}_1\) and \(\mathbf{b}_2\), respectively.

Note

The matrix \(\mathbf{D}\) is one-based indexing. Thus,

\[\begin{split}\mathbf{D} = \begin{bmatrix} | & | & | \\ \mathbf{d}_1 & \mathbf{d}_2 & \mathbf{d}_3 \\ | & | & | \end{bmatrix} = \begin{bmatrix} d_{11} & d_{12} & d_{13} \\ d_{21} & d_{22} & d_{23} \\ d_{31} & d_{32} & d_{33} \end{bmatrix}\end{split}\]

Because \(\mathbf{D}\) and \(\mathbf{r}_i\) are available and have a simple form, we can compute \(\mathbf{K}_2\) directly using \(a_i = 0.5\):

\[\begin{split}\mathbf{K}_2 = \frac{1}{2}\begin{bmatrix} d_{11} - d_{22} & d_{21} + d_{12} & d_{31} & -d_{32} \\ d_{21} + d_{12} & d_{22} - d_{11} & d_{32} & d_{31} \\ d_{31} & d_{32} & -d_{11} - d_{22} & d_{12} - d_{21} \\ -d_{32} & d_{31} & d_{12} - d_{21} & d_{11} + d_{22} \end{bmatrix}\end{split}\]

The sought quaternion, \(\mathbf{q}\), is obtained by computing the eigenvector of \(\mathbf{K}_2\) that belongs to the eigenvalue 1.

Version 2

If the given DCM is imprecise but still orthogonal, we can use either two or three pairs and obtain the same results.

However, the quaternion obtained when using three pairs yields the DCM that is the closest orthogonal matrix.

Re-defining our cost function as:

\[L(\mathbf{D}) = \sum_{i=1}^{k}a_i - \mathrm{tr}(\mathbf{DB})^T\]

where:

\[\mathbf{B} = \sum_{i=1}^{k}a_i\mathbf{b}_i\mathbf{r}_i^T\]

In this case, the matrix \(\mathbf{D}_{\mathrm{orth}}\) that minimizes the cost function \(L(\mathbf{D})\) is the same matrix that maximizes \(\mathrm{tr}(\mathbf{DB})^T\), and is computable as:

\[\mathbf{D}_{\mathrm{orth}} = \mathbf{B}(\mathbf{B}^T\mathbf{B})^{-\frac{1}{2}}\]

Adding a third pair of vectors similar to the first two:

\[\mathbf{r}_3^T = \begin{bmatrix} 0 & 0 & 1 \end{bmatrix}\]

we easily redefine the matrix \(\mathbf{B}\):

\[\begin{split}\begin{array}{rcl} \mathbf{B} &=& \frac{1}{3} \mathbf{b}_1\mathbf{r}_1^T + \frac{1}{3} \mathbf{b}_2\mathbf{r}_2^T + \frac{1}{3} \mathbf{b}_3\mathbf{r}_3^T \\ &=& \frac{1}{3}\begin{bmatrix} \mathbf{d}_1 & \mathbf{d}_2 & \mathbf{d}_3 \end{bmatrix} \\ &=& \frac{1}{3}\mathbf{D} \end{array}\end{split}\]

Therefore,

\[\mathbf{D}_{\mathrm{orth}} = \mathbf{D}(\mathbf{D}^T\mathbf{D})^{-\frac{1}{2}}\]

Using only two vectors would still yield an optimal quaternion, but it would not correspond to the closest orthogonal matrix of the given imprecise mathbf{D}.

Thus, \(\mathbf{D}_{\mathrm{orth}}\) is the closest orthogonal matrix of the given imprecise \(\mathbf{D}\) that solves Wahba’s problem.

From here, we define \(\mathbf{K}_3\) as:

\[\begin{split}\mathbf{K}_3 = \frac{1}{3}\begin{bmatrix} d_{11} - d_{22} - d_{33} & d_{21} + d_{12} & d_{31} + d_{13} & d_{23} - d_{32} \\ d_{21} + d_{12} & d_{22} - d_{11} - d_{33} & d_{32} + d_{23} & d_{31} - d_{13} \\ d_{31} + d_{13} & d_{32} + d_{23} & d_{33} - d_{11} - d_{22} & d_{12} - d_{21} \\ d_{23} - d_{32} & d_{31} - d_{13} & d_{12} - d_{21} & d_{11} + d_{22} + d_{33} \end{bmatrix}\end{split}\]

And, similarly to version 1, the sought quaternion, \(\mathbf{q}\), is the eigenvector of \(\mathbf{K}_3\) that belongs to the eigenvalue 1.

Version 3

If a given DCM is not quite orthogonal, the results will not be correct. If two resulting quaternions are converted to DCM, the two DCMs will be orthogonal because this is an inherent quality of the expression of the DCM in terms of the corresponding quaternion.

Therefore, for a given non-orthogonal DCM, we re-use the same matrix \(\mathbf{K}_3\).

The sought quaternion, \(\mathbf{q}\), is the eigenvector of \(\mathbf{K}_3\) that belongs to its largest eigenvalue, \(\lambda_{\mathrm{max}}\).

The main benefit of this version is that the computed quaternion yields the closest orthogonal matrix to the given DCM.

Finally, the extraction of the quaternion from the \(\mathbf{K}\) matrix can be done either using the QUEST and similar algorithms or, preferably, using a known standard algorithm for computing the eigenvalues and eigenvectors of a real symmetric matrix.

Parameters:

• dcm (numpy.ndarray) – 3-by-3 Direction Cosine Matrix.

• version (int, default: 3) – Version used to compute the Quaternion. Options are 1, 2 or 3.

Returns:

q – Quaternion.

Return type:

numpy.ndarray