6
$\begingroup$

I'am working with an LSM6DSO32, so I'am starting with Kalman filter, everything works but with Euler angle I got gimbal lock I think... But if my pitch angle approach to 90° my roll angle jump and I get a bad value and my pitch is like blocked around 90°.

So I searched and I foud this phenomenon "Gimbal lock" after some other researches I have read I should use Quaternions to avoid this Gimbal lock. I found this code that implement MadgwickAHRS algo who use Quaternions here

But if I use computeAngles, this function return Euler angles I have again Gimbal problem. So my question is, how can I only use Quaternions without Euler transformation to compute my roll and pitch angles? My objective is to know if my roll or my pitch angles has move to 90 or -90° aboyt my start position. For example, if my start position is roll 20° and pitch 10°, I would like to detect if my roll is 110° or -70° and if my pitch is 100° or -80°

How can I solve my problem with only q0, q1, q2, q3 values ?

I found this article here I've tried the solution for gimbal lock, but when I set my roll to 0 and compute yaw with 2atan2(q1, q0) for pitch equals PI/2 the value I got isn't good, yaw move a lot...

$\endgroup$
3
  • $\begingroup$ When dealing with quaternions (or rotation matrices) it is easier to consider how certain vectors have rotated instead of the composition of yaw, pitch and roll rotations. With your pitch and roll bounds do you mean that your local up vector, which does not rotate when just yawing, has rotated more than 90° compared to your initial orientation? $\endgroup$
    – fibonatic
    Jun 27, 2021 at 22:15
  • $\begingroup$ Roll, pitch are Euler angles. If that is the desired output, you are going to have gimbal lock when pitch = 90. $\endgroup$ Feb 20, 2022 at 4:56
  • $\begingroup$ Another good site for geometric conversions: euclideanspace.com/maths/geometry/rotations/conversions/… $\endgroup$
    – Ben
    Apr 1, 2022 at 12:59

2 Answers 2

6
$\begingroup$

Quaternions are a more efficient way of storing the orientation matrix of a frame.

I use the vector-scalar convention for quaternions (3+1 = 4 quantities) and have defined the following utility functions

  • Quaternion definition $$q=\begin{pmatrix}\vec{v}\\ s \end{pmatrix}$$
  • Quatenion from rotation axis $\hat{z}$ and angle $\theta$ $$q=\begin{pmatrix}\hat{z}\sin\left(\frac{\theta}{2}\right)\\ \cos\left(\frac{\theta}{2}\right) \end{pmatrix}$$
  • Identity quaternion $$q=\begin{pmatrix}\vec{0}\\ 1 \end{pmatrix}$$
  • Magnitude $$ \|q \| = \sqrt{ \vec{v} \cdot \vec{v} + s^2 }$$
  • Unit quaternion (to represent a rotation) $$ {q} = \frac{1}{\sqrt{ \vec{v} \cdot \vec{v} + s^2 }} \begin{pmatrix} \vec{v}\\ s \end{pmatrix}$$
  • Inverse quaternion $$q^{-1} = \frac{1}{\vec{v} \cdot \vec{v} + s^2}\begin{pmatrix}-\vec{v}\\ s \end{pmatrix}$$
  • Sequence of two rotations $$q_1 q_2 = \begin{pmatrix}s_{1}\vec{v}_{2}+s_{2}\vec{v}_{1}+\vec{v}_{1}\times\vec{v}_{2}\\ s_{1}s_{2}-\vec{v}_{1}\cdot\vec{v}_{2} \end{pmatrix}$$
  • Transform vector $\vec{p}$ by quatertion rotation $q$ $$ \vec{p}' = \vec{p}+2s\left(\vec{v}\times\vec{p}\right)+2\left(\vec{v}\times\left(\vec{v}\times\vec{p}\right)\right)$$
  • Inverse Transform vector $\vec{p}'$ by quatertion rotation $q$ $$ \vec{p} = \vec{p}' - 2s\left(\vec{v}\times\vec{p}'\right)+2\left(\vec{v}\times\left(\vec{v}\times\vec{p}'\right)\right)$$
  • Rodrigues 3×3 rotation matrix from quatenion $$\mathbf{R}=\mathbf{1}+2s[\vec{v}\times]+2[\vec{v}\times][\vec{v}\times]$$ where $[\vec{v}\times] = \begin{vmatrix}0 & -v_z & v_y \\ v_z & 0 & -v_x \\ -v_y & v_x & 0 \end{vmatrix} $ is the skew symmetric cross product operator matrix.
  • Inverse Rodrigues 3×3 rotation matrix from quatenion $$\mathbf{R}^\top=\mathbf{1}-2s[\vec{v}\times]+2[\vec{v}\times][\vec{v}\times]$$
  • Quaternion from 3×3 rotation matrix $\mathbf{R}$ $$\begin{aligned}s & =\frac{1}{2}\sqrt{\frac{\left(\mathbf{R}_{32}-\mathbf{R}_{23}\right)^{2}+\left(\mathbf{R}_{13}-\mathbf{R}_{31}\right)^{2}+\left(\mathbf{R}_{21}-\mathbf{R}_{12}\right)^{2}}{3-\mathbf{R}_{11}-\mathbf{R}_{22}-\mathbf{R}_{33}}}\\ \vec{v} & =\frac{1}{4s}\begin{pmatrix}\mathbf{R}_{32}-\mathbf{R}_{23}\\ \mathbf{R}_{13}-\mathbf{R}_{31}\\ \mathbf{R}_{21}-\mathbf{R}_{12} \end{pmatrix} \end{aligned}$$
  • Time derivative of quaternion subject to rotational velocity vector $\vec{\omega}$ $$ \dot{q} = \frac{1}{2}\begin{pmatrix}s\vec{\omega}+\vec{\omega}\times\vec{v}\\ -\vec{\omega} \cdot \vec{v} \end{pmatrix}$$ Remeber $\vec{v}$ is the vector part of the quaternion and not a velocity vector.
  • Rotation velocity vector from quaternion and its time derivative $$\begin{pmatrix}\vec{\omega}\\ 0 \end{pmatrix} = 2\begin{pmatrix}s\dot{\vec{v}}-\vec{v}\dot{s}+\vec{v}\times\dot{\vec{v}}\\ s\dot{s}+\vec{v} \cdot \dot{\vec{v}} \end{pmatrix}$$

All of the above functions can be used in the simulation and modeling of rigid bodies by keeping track of each body's orientation $q$ in the configuration space, and doing a time integration with $q \rightarrow q + \dot{q} \Delta t$, just like you would integrate position $\vec{r} \rightarrow \vec{r} + \dot{\vec{r}} \Delta t$

No Euler angles needed for the model. If you need to extract Euler angles, then convert the quaternion into a rotation matrix, and extract from there.

$\endgroup$
2
  • $\begingroup$ Do you have an example of this "If you need to extract Euler angles, then convert the quaternion into a rotation matrix, and extract from there." @John Alexiou ? $\endgroup$
    – simon
    Dec 19, 2022 at 15:40
  • 1
    $\begingroup$ @simon - the step above shows how to extract the rotation matrix ${\bf R}$. Then search online for a rotation matrix to euler angles for a solution that matches your programming environment and euler angle convention. This is beyond the scope of this question. $\endgroup$ Dec 19, 2022 at 19:17
0
$\begingroup$

If you define your tested conditions in terms of euler angles, you're going to have to use euler angles. I would suggest finding a way to reframe your question not in terms of euler angles but by the quantities of change.

It's not clear what you mean by q0 - q4 are those the individual values of one quaternion, or 4 different quaternions?

It seems like you should have an initial quaternion and a final quaternion. You can then compute the difference, and likely provide logic that thresholds on properties of that resultant quaternion. Such as the resultant angle is 90 degrees from the original. Or maybe the angle around a specific axis like asked here

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.