1
$\begingroup$

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} $$

$\endgroup$

1 Answer 1

1
$\begingroup$

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. $$

$\endgroup$
4
  • $\begingroup$ This is what I would have recommended in an answer, and this actually follows through with the solution of the simultaneous equations. I see that the values in the example at the end of the question imply that $DB-AC<0,$ and the formula for $r$ derived in the question is equivalent to this answer in the case that $DB-AC<0$ and $\cos Y>0.$ In the case where $DB-AC<0$ and $\cos Y<0$ this predicts that the formula in the question will be off by $180^\circ,$ exactly as observed. $\endgroup$
    – David K
    Feb 20, 2023 at 20:33
  • $\begingroup$ Thanks for the answer. I'll take a close look to understand it. I indeed suspected it had something to do with tan(yaw). But I am surprised that the choice of the equation to pick in the end is strictly dependant on the constants A, B, C, and D, and not dependant on Y. $\endgroup$ Feb 21, 2023 at 4:16
  • $\begingroup$ To some degree $r$ does not depend on $Y$. Note that $\sin^2Y+\cos^2Y=1=(A\sin r+B\cos r)^2+(C\cos r+D\sin r)^2$ requires $$ \frac{A^2+B^2+C^2+D^2}{2}+(AB+CD)\sin(2r)+\frac{-A^2+B^2+C^2-D^2}{2}\cos(2r)=1 $$ which could probably solved for $r$ (although the 180$^\circ$ ambiguity remains). $\endgroup$ Feb 21, 2023 at 17:51
  • $\begingroup$ I finally got the time to look at all this in more details and indeed switching from tan(yaw) to sin(yaw)/cos(yaw) solved all the 185 millions practical cases of my system (all combinations of A, B, C, D and yaw across a range making sense for my system). Almost half of these test vectors were failing before. I'm still not choosing between 2 equations though... It seems that keeping the signs of sin Y and cos Y did the trick. $\endgroup$ Feb 25, 2023 at 4:30

You must log in to answer this question.

Not the answer you're looking for? Browse other questions tagged .