How do I express three desired basis vectors as a quaternion?

418 Views Asked by At

I am creating a simulation that relies on the controller developed in the paper "Automatic Re-Initialization and Failure Recovery for Aggressive Flight with a Monocular Vision-Based Quadrotor" which can be found here: http://rpg.ifi.uzh.ch/docs/ICRA15_Faessler.pdf

On page 5 of the paper it states "Now, the full desired attitude $\boldsymbol{q}_{des}$ can be built from the three desired body axes $\boldsymbol{e}_{x,des}^{B}$, $\boldsymbol{e}_{y,des}^{B}$, $\boldsymbol{e}_{z,des}^{B}$."

How do I build a quaternion from the three basis vectors?

1

There are 1 best solutions below

2
On

This is known as Wahba's problem, which is stated for rotation matrices as

$$ \min_{R\in SO(3)} \sum_{k=1}^N a_k \left\|w_k - R\,v_k\right\|^2, \tag{1} $$

where $v_k$ and $w_k$ are vectors in reference frame $\mathcal{A}$ and $\mathcal{B}$ respectively and the minimizing $R$ should be the best approximation of the rotation from frame $\mathcal{A}$ to $\mathcal{B}$. When reconstructing the attitude of something the vectors $v_k$ and $w_k$ can both either be measured or known directions. For example the directions of gravity would be known in the world frame and could be measured in the body frame of a drone. It can be noted that $(1)$ does not have a unique solution when the rank of $\begin{bmatrix}v_1 & v_2 & \cdots & v_N\end{bmatrix}$ and the rank of $\begin{bmatrix}w_1 & w_2 & \cdots & w_N\end{bmatrix}$ are both less than two.

A similar optimization problem could also be formulated for quaternions by replacing $R\,v_k$ in $(1)$ with the vector $v_k$ rotated by a unit quaternion. There are also multiple algorithms known which can solve Wahba's problem for quaternions, such as QUEST, ESOQ and FOMA.