0
$\begingroup$

Not exactly a robotics based question but mechanics is involved. I have a wearable device that gives output in Quaternions which I can read serially via Labview. My task is to develop a threshold based fall detection system based on these values which I am not familiar with.

Here is a sample data I read from the device

id: 4 distance: 1048 q0: 646 q1: -232 q2: -119 q3: 717 

I was able to find the Euler angles from the quaternions. I obtain a Rotation matrix from the Quaternion. From the rotation matrix, I derive the Roll, Pitch and Yaw. The coordinate system is North-East-Down. But my Pitch angle remains at positive or negative 90 degree. The fact is I didn't write the conversion code . I am attaching the

Rotation MatrixRPYAnglefrmRotMatrx.

Please have a look at the code and help me if you could

$\endgroup$
  • $\begingroup$ I don't understand. The whole point of using Quaternions is to avoid the singularities associated with Euler angles. Also, this comparison (angle == 90) may never occur due to floating point issue. Check this out 0.1 + 0.1 + 0.1 == 0.3 which should return 1 but in Matlab, it will return 0!!. $\endgroup$ – CroCo Oct 13 '16 at 11:28
  • $\begingroup$ @CroCo, I agree that quaternions must be used in a simulation to avoid the singularities associated with Euler angles. It is, however, useful to also calculate the Euler angles from the quaternions (or, from the Rotation matrix) because Euler angles are much easier for humans to visualize :-) $\endgroup$ – Christo Oct 14 '16 at 11:20
2
$\begingroup$

The problem is that the quaternions given in

id: 4 distance: 1048 q0: 646 q1: -232 q2: -119 q3: 717

are not normalized. In fact, $$ \|\textbf{q}\| = \sqrt{q_0^2+q_1^2+q_2^2+q_3^2} = 999.6950 $$ I suspect the wearable device gives the quaternions using fixed point numbers that must be scaled by dividing by 1000. Another problem is that the quaternions are only given to three significant digits, so it will be better to calculate the quaternion norm, $\|q\|$, first using the equation above, and then normalize the quaternions using $q_0\leftarrow q_0/\|\textbf{q}\|$, $q_1\leftarrow q_1/\|\textbf{q}\|$, $q_2\leftarrow q_2/\|\textbf{q}\|$, $q_3\leftarrow q_3/\|\textbf{q}\|$. Then you can directly calculate the Euler angles from the quaternions using \begin{eqnarray*} \phi &=& \tan^{-1}\left(\frac{2(q_2q_3+q_0q_1)}{q_0^2-q_1^2-q_2^2+q_3^2}\right) \\ \theta &=& -\sin^{-1}\left(2q_1q_3-2q_0q_2\right) \\ \psi &=& \tan^{-1}\left(\frac{2(q_1q_2+q_0q_3)}{q_0^2+q_1^2-q_2^2-q_3^2}\right) \end{eqnarray*} without going through the intermediate step of calculating the rotation matrix. Also make sure to use the four-quadrant arctan function, atan2(), to calculate $\phi$ and $\psi$ to obtain the correct quadrant. Here, $\phi$ is the roll angle, $\theta$ the pitch angle, and $\psi$ the yaw angle; all three angles in radians. Simple multiply by $180/\pi$ to obtain the angles in degrees. Your answer should be \begin{eqnarray*} \phi &=& -28.5815^\circ \\ \theta &=& 10.3144^\circ \\ \psi &=& 93.3298^\circ \end{eqnarray*} If you repeat the above calculation without first normalizing the quaternions, you will obtain the correct roll and yaw angles, but a complex pitch angle: \begin{eqnarray*} \phi &=& -28.5815^\circ \\ \theta &=& (90-j732.7)^\circ \\ \psi &=& 93.3298^\circ \end{eqnarray*}

Hope this helps!

$\endgroup$
  • $\begingroup$ This is a terrific answer! Good job spotting the problem with the sensor data (not normalized). $\endgroup$ – Chuck Oct 14 '16 at 12:42

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

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