I have this equation that allows me to compute the yaw Tait-Bryan Euler angle from a general 3D rotation matrix in my system (see More Info below) :
$$\text{yaw} = {\rm atan2}(-0.01775\sin(RZ – 90°) + 0.9997\cos(RZ – 90°) , -0.01714\cos(RZ – 90°) – 0.99924\sin(RZ – 90°))$$
For example: if $RZ = -136.01355°$, I get $\text{yaw} = -135.0°$ which is what I expect given my system.
Now what I really want is to find $RZ$ given a known $\text{yaw}$ value. But for some reason this does not always work. I sometimes am $\pm 180^\circ$ off. Here is how I re-arranged the equation:
$$
\begin{align}
{\rm atan2}(A\sin(r) + B\cos(r), C\cos(r) + D\sin(r)) &= Y \\\\
\frac{A\sin(r) + B\cos(r)}{C\cos(r) + D\sin(r)} &= \tan(Y) \\\\
A\sin(r) + B\cos(r) &= \tan(Y)C\cos(r) + \tan(Y)D\sin(r) \\\\
(A – \tan(Y)D)\sin(r) + (B – \tan(Y)C)\cos(r) &= 0 \\\\
(A – \tan(Y)D)\tan(r) + (B – \tan(Y)C) &= 0 \\\\
(A – \tan(Y)D)\tan(r) &= \tan(Y)C – B \\\\
\tan(r) &= \frac{\tan(Y)C – B}{A – \tan(Y)D} \\\\
r &= {\rm atan2}(\tan(Y)C – B, A – \tan(Y)D) \\\\
\end{align}
$$
With my system's values (see More Info below):
$$RZ = {\rm atan2}(-0.01714\tan(\text{yaw}) – 0.9997 , 0.99924\tan(\text{yaw}) – 0.01775) + 90°$$
So for $\text{yaw} = 135°$, I get $RZ = 43.98645°$ which is exactly $180°$ more than the expected value of $-136.01355°$ (i.e. it looks like I need to subtract $180°$ from my result).
It seems that for yaw angles in the range $-90° < \text{yaw} < 90°$, RZ is good; for $\text{yaw} \le -90°$, I need to do $RZ – 180°$; and for $\text{yaw} \ge 90°$, I need to do $RZ + 180°$.
Does this make sense? Is my math ok? What is the explanation for it? Is it somehow related to $\tan(\text{yaw})$? Is there a better solution that gives the answer directly? If not, what would be the correct algorithm to decide when to add/subtract $180°$ and at which step should it be done?
More info about my system
General 3D rotation matrix:
$$
R =
\begin{bmatrix}
R_{11} & R_{12} & R_{13} \\
R_{21} & R_{22} & R_{23} \\
R_{31} & R_{32} & R_{33} \\
\end{bmatrix}
$$
From this rotation matrix, I know I can get the yaw Tait-Bryan Euler angle with this:
$$\text{yaw} = {\rm atan2}(R_{21}, R_{11})$$
In my system I have:
$$R_{21} = A\sin(r) + B\cos(r)$$
$$R_{11} = C\cos(r) + D\sin(r)$$
with:
$$
\begin{align}
r &= RZ – 90° \\\\
A &= -0.01775 \\\\
B &= 0.9997 \\\\
C &= -0.01714 \\\\
D &= -0.99924 \\\\
\end{align}
$$
Best Answer
In the equation $$ \frac{A\sin r+B \cos r}{C \cos r +D \sin r}=\tan Y $$ a substitution $r\to r+180^\circ$ flips signs of both $\sin r$ and $\cos r$ and such that the ratio on the left hand side stays the same. That means starting from that equation the $\tan Y$ does not carry the full information on the solutions $r$. The remedy is to keep separately track of the signs of $A\sin r+B\cos r$ and $C\cos r+D\sin r$: $$ A \sin r+B\cos r = \sin Y; \quad C\cos r +D\sin r = \cos Y $$ $$ \left(\begin{array}{cc}A & B \\ D & C\end{array}\right)\cdot \left(\begin{array}{c}\sin r\\ \cos r\end{array}\right) = \left(\begin{array}{c}\sin Y\\ \cos Y\end{array}\right) $$ $$ \sin r = \frac{B\cos Y-C \sin Y}{DB-AC};\quad \cos r = \frac{D\sin Y-A \cos Y}{DB-AC} $$ $$ r=atan2(\frac{B\cos Y-C \sin Y}{DB-AC}, \frac{D\sin Y-A \cos Y}{DB-AC}). $$ Canceling the common $DB-AC$ and keeping track of the correct branches means $$ r = \left\{ \begin{array}{ll} atan2( B\cos Y-C\sin Y, D\sin Y-A \cos Y);& DB-AC>0; \\ atan2( -B\cos Y+C\sin Y, -D\sin Y+A \cos Y);& DB-AC<0 \\ \end{array} \right.. $$ One could also divide both terms through $\cos Y$ to get $$ r = \left\{ \begin{array}{ll} atan2( \frac{B-C \tan Y}{DB-AC}, \frac{D\tan Y-A}{DB-AC});& \cos Y>0; \\ atan2(-\frac{B-C \tan Y}{DB-AC}, -\frac{D\tan Y-A}{DB-AC}) ;& \cos Y<0 \\ \end{array} \right. $$