Solving nonlinear equations for yaw/pitch/roll given rotor/quaternio

[chogg]

I've been doing linear algebra for so long, that I've become quite a dunce at solving nonlinear systems of equations.

tl;dr: Can anyone suggest a fruitful plan of attack for the following system of equations?

<br /> \begin{align}<br /> \cos\phi_1 \cos\phi_2 \cos\phi_3 + \sigma \sin\phi_1 \sin\phi_2 \sin\phi_3 = {}&amp; K_0 \\<br /> \sin\phi_1 \cos\phi_2 \cos\phi_3 - \sigma \cos\phi_1 \sin\phi_2 \sin\phi_3 = {}&amp; K_1 \\<br /> \cos\phi_1 \sin\phi_2 \cos\phi_3 + \sigma \sin\phi_1 \cos\phi_2 \sin\phi_3 = {}&amp; K_2 \\<br /> \cos\phi_1 \cos\phi_2 \sin\phi_3 - \sigma \sin\phi_1 \sin\phi_2 \cos\phi_3 = {}&amp; K_3<br /> \end{align}<br />

This is 4 equations in 3 unknowns, but they are not independent; it turns out that K_0^2 + K_1^2 + K_2^2 + K_3^2 = 1 (as you can easily verify).

Details below...


The motivating problem: I've applied three consecutive rotations in 3D; the planes of these rotations are mutually orthogonal. These equations give the components of the rotor (K_0, K_1, K_2, K_3) in terms of the half-angles of rotation (\phi_i, \phi_2, \phi_3). \sigma = \pm 1 is a constant which specifies the handedness of the bivector triple. (I wanted to be general, but at this point I'd accept a method which just picks a convenient handedness.)

Below, I'll list the approaches I've tried in case anyone's interested. I just wanted to put the tl;dr version up at the top.

I'm aware that solutions exist online, but I don't find them satisfying because they don't tell me how to solve something like this. (At least, the ones I've seen.) And I'm taking this as an opportunity to brush up some rusty skills.

Approach 1: Squaring and canceling

I thought it might be helpful to square the equations. That way, I could consider S_i \equiv \sin^2(\phi_i) as my unknowns, replace \cos^2(\phi_i) with (1 - S_i), and forget about the trig for a while. There are those pesky linear terms, but these equations have a very specific structure which helps: the linear term is always \pm 2 \sigma \cos\phi_1 \cos\phi_2 \cos\phi_3 \sin\phi_1 \sin\phi_2 \sin\phi_3. So we can cancel it out and we're left with only squared trigonometric functions.

Unfortunately, the remaining three equations are not independent. No matter how I combine them, all the variables always drop out and I'm left with \sum K_i^2 = 1, which I already knew.

Approach 2: Adding equations; using trig addition formulae

If I add the equations for K_1 and K_3, and also the equations for K_0 and K_2, I get the following:

<br /> \begin{align}<br /> \sin(\phi_1 + \phi_3)(\cos\phi_2 - \sigma \sin\phi_2) = {}&amp; K_1 + K_3 \\<br /> \cos(\phi_1 - \sigma\phi_3)(\cos\phi_2 + \sin\phi_2) = {}&amp; K_0 + K_2<br /> \end{align}<br />

If I bite the bullet and pick \sigma = -1 (giving up for now on my general solution), these two equations divide quite nicely to give
<br /> \tan(\phi_1 + \phi_3) = \frac{K_1 + K_3}{K_0 + K_2}<br />

This looks pretty promising! I can take the \arctan of both sides to get a linear equation in \phi_1 and \phi_3. If I get two more equations like this, I'll have a linear system -- and there are two more ways to combine these four equations!

Unfortunately, those other two ways don't look very fruitful to me. Even with the same choice of \sigma = -1, I get:
<br /> \begin{align}<br /> \cos\phi_1 \sin(\phi_3 + \phi_2) - \sin\phi_1\sin(\phi_3 - \phi_2) = {}&amp; K_2 + K_3 \\<br /> \cos\phi_1 \cos(\phi_3 - \phi_2) + \sin\phi_1\cos(\phi_3 + \phi_2) = {}&amp; K_0 + K_1<br /> \end{align}<br />
It's just not clear to me how I would change these two equations into a linear equation for \phi_2 and \phi_3.

Other ideas?

I'd really appreciate any guidance. I've been somewhat obsessing over this problem. :)

 

[chogg]

Here are the magic combinations of the K's:

<br /> \begin{align}<br /> \cos\theta_2 \sin\theta_1 =&amp; 2(K_0 K_1 + \sigma K_2 K_3) \\<br /> \cos\theta_2 \cos\theta_1 =&amp; K_0^2 - K_1^2 - K_2^2 + K_3^2 \\<br /> \sin\theta_2 =&amp; 2(K_0 K_2 - \sigma K_1 K_3) \\<br /> \cos\theta_2 \sin\theta_3 =&amp; 2(K_0 K_3 + \sigma K_2 K_1) \\<br /> \cos\theta_2 \cos\theta_3 =&amp; K_0^2 + K_1^2 - K_2^2 - K_3^2<br /> \end{align}<br />

If we constrain \theta_2 to (-\pi, \pi), then \sin\theta_2 determines it uniquely. And this is a reasonable approach, since allowing higher angles would only lead to redundancy anyway. The other angles are fully determined by the signs of \sin\theta_i and \cos\theta_i as long as \cos\theta_2 \ne 0 (a special case). This leaves us with the following explicit solutions for the angles:

<br /> \begin{align}<br /> \theta_1 &amp;= \text{atan2}(2(K_0 K_1 + \sigma K_2 K_3),<br /> K_0^2 - K_1^2 - K_2^2 + K_3^2) \\<br /> \theta_2 &amp;= \arcsin(2(K_0 K_2 - \sigma K_1 K_3)) \\<br /> \theta_3 &amp;= \text{atan2}(2(K_0 K_3 + \sigma K_2 K_1),<br /> K_0^2 + K_1^2 - K_2^2 - K_3^2)<br /> \end{align}<br />

Inspiration came from considering the rotated bivectors. Note that the first rotation leaves its own bivector unchanged. So, consider the components of that bivector after the full rotation -- this gives 3 equations, which can only depend on the second two angles (greatly simplifying the equations). Each other rotated bivector yielded one useful equation, for the total of 5 K-combinations I listed at the beginning of this post.

I didn't end up learning any general techniques for attacking nonlinear equations, but I don't mind, since this problem is chock-full of beautiful symmetries.