Pitch angle is either +90° or -90°

Question

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

.

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

Answer 1

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!