Sarabandi’s Method

ahrs.common.orientation.sarabandi(dcm: ndarray, eta: float = 0.0) → ndarray

Quaternion from a Direction Cosine Matrix using Sarabandi’s method [ST19].

A rotation matrix \(\mathbf{R}\) can be expressed as:

\[\begin{split}\mathbf{R} = \begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{bmatrix}\end{split}\]

A quaternion \(\mathbf{q}\) describing the same rotation can be expressed as:

\[\begin{split}\mathbf{q} = \begin{bmatrix} q_w \ q_x \ q_y \ q_z \end{bmatrix} = \begin{bmatrix} \cos (\frac{\theta}{2}) \\ n_x \sin (\frac{\theta}{2}) \\ n_y \sin (\frac{\theta}{2}) \\ n_z \sin (\frac{\theta}{2}) \end{bmatrix}\end{split}\]

where \(\theta\) is the rotation angle, and \(\mathbf{n}\) is the unitary rotation axis. The quaternion is also unitary, i.e., \(\|\mathbf{q}\| = 1\).

The rotation matrix \(\mathbf{R}\) can be expressed in terms of the quaternion \(\mathbf{q}\) as:

\[\begin{split}\mathbf{R} = \begin{bmatrix} 1 - 2q_y^2 - 2q_z^2 & 2(q_xq_y - q_zq_w) & 2(q_xq_z + q_yq_w) \\ 2(q_xq_y + q_zq_w) & 1 - 2q_x^2 - 2q_z^2 & 2(q_yq_z - q_xq_w) \\ 2(q_xq_z - q_yq_w) & 2(q_yq_z + q_xq_w) & 1 - 2q_x^2 - 2q_y^2 \end{bmatrix}\end{split}\]

As with Shepperd’s method, we build a system of linear equations with \(q_w\), \(q_x\), \(q_y\) and \(q_z\):

\[\begin{split}\begin{array}{rcl} 4q_w^2 &=& 1 + r_{11} + r_{22} + r_{33} \\ 4q_x^2 &=& 1 + r_{11} - r_{22} - r_{33} \\ 4q_y^2 &=& 1 - r_{11} + r_{22} - r_{33} \\ 4q_z^2 &=& 1 - r_{11} - r_{22} + r_{33} \\ 4q_yq_z &=& r_{23} + r_{32} \\ 4q_xq_z &=& r_{31} + r_{13} \\ 4q_xq_y &=& r_{12} + r_{21} \\ 4q_wq_x &=& r_{32} - r_{23} \\ 4q_wq_y &=& r_{13} - r_{31} \\ 4q_wq_z &=& r_{21} - r_{12} \end{array}\end{split}\]

Clearing for \(q_w\), we get:

\[q_w = \frac{1}{2}\sqrt{1 + r_{11} + r_{22} + r_{33}}\]

We see that \(\mathrm{trace}(\mathbf{R}) = r_{11}+r_{22}+r_{33} = 2\cos\theta + 1\).

This becomes ill-conditioned when \(\theta \rightarrow \pi\), and \(q_w\) can even become negative due to rounding errors, which is not allowed for our unit quaternion.

To obtain a more robust solution, we involve the off-diagonal elements of the rotation matrix.

Using the system of linear equations to substitute \(q_w\), \(q_x\), \(q_y\) and \(q_z\) in \(q_w^2 + q_x^2 + q_y^2 + q_z^2 = 1\) we get:

\[\frac{1+r_{11}+r_{22}+r_{33}}{4} + \Bigg(\frac{r_{32}-r_{23}}{4q_w}\Bigg)^2 + \Bigg(\frac{r_{13}-r_{31}}{4q_w}\Bigg)^2 + \Bigg(\frac{r_{21}-r_{12}}{4q_w}\Bigg)^2 = 1\]

Solving for \(q_w\):

\[q_w = \frac{1}{2}\sqrt{\frac{(r_{32}-r_{23})^2 + (r_{13}-r_{31})^2 + (r_{21}-r_{12})^2}{3 - r_{11} - r_{22} - r_{33}}}\]

This new definition of \(q_w\) is now ill-conditioned when \(\theta \rightarrow 0\). Thus, both definitions can be seen as complementary.

Now we simply establish a threshold for the trace of the rotation matrix. This threshold is easily defined from the terms inside the square root.

\[\begin{split}q_w = \left\{ \begin{array}{lc} \frac{1}{2}\sqrt{1+r_{11}+r_{22}+r_{33}} & \mathrm{if}\; r_{11}+r_{22}+r_{33} > \eta \\ \frac{1}{2}\sqrt{\frac{(r_{32}-r_{23})^2 + (r_{13}-r_{31})^2 + (r_{21}-r_{12})^2}{3-r_{11}-r_{22}-r_{33}}} & \mathrm{otherwise} \end{array} \right.\end{split}\]

Repeating the same process for the other elements of the quaternion:

\[\begin{split}\begin{array}{rcl} q_x &=& \left\{ \begin{array}{lc} \frac{1}{2}\sqrt{1+r_{11}-r_{22}-r_{33}} & \mathrm{if}\; r_{11}-r_{22}-r_{33} > \eta \\ \frac{1}{2}\sqrt{\frac{(r_{32}-r_{23})^2 + (r_{12}+r_{21})^2 + (r_{31}+r_{13})^2}{3-r_{11}+r_{22}+r_{33}}} & \mathrm{otherwise} \end{array} \right.\\\\ q_y &=& \left\{ \begin{array}{lc} \frac{1}{2}\sqrt{1-r_{11}+r_{22}-r_{33}} & \mathrm{if}\; -r_{11}+r_{22}-r_{33} > \eta \\ \frac{1}{2}\sqrt{\frac{(r_{13}-r_{31})^2 + (r_{12}+r_{21})^2 + (r_{23}+r_{32})^2}{3+r_{11}-r_{22}+r_{33}}} & \mathrm{otherwise} \end{array} \right.\\\\ q_z &=& \left\{ \begin{array}{lc} \frac{1}{2}\sqrt{1-r_{11}-r_{22}+r_{33}} & \mathrm{if}\; -r_{11}-r_{22}+r_{33} > \eta \\ \frac{1}{2}\sqrt{\frac{(r_{21}-r_{12})^2 + (r_{31}+r_{13})^2 + (r_{23}+r_{32})^2}{3+r_{11}+r_{22}-r_{33}}} & \mathrm{otherwise} \end{array} \right. \end{array}\end{split}\]

Finally, if \(q_w\) is positive, we redefine the sign of the quaternion elements, with the signs of \(r_{32}-r_{23}\), \(r_{13}-r_{31}\), and \(r_{21}-r_{12}\) assigned to \(q_x\), \(q_y\), and \(q_z\), respectively.

Parameters:

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

• eta (float,d efault: 0.0) – Threshold.

Returns:

q – Quaternion.

Return type:

numpy.ndarray