Solve a linear system of three trigonometric equations in three angles

Question

You are given the linear trigonometric system of equations described by:

$ a_1 \cos(x) + a_2 \cos(y) + a_3 \cos(z) + a_4 \sin(x) + a_5 \sin(y) + a_6 \sin(z) = d \tag{1} $

$ b_1 \cos(x) + b_2 \cos(y) + b_3 \cos(z) + b_4 \sin(x) + b_5 \sin(y) + b_6 \sin(z) = e \tag{2} $

$ c_1 \cos(x) + c_2 \cos(y) + c_3 \cos(z) + c_4 \sin(x) + c_5 \sin(y) + c_6 \sin(z) = f \tag{3}$

where $a_i, b_i, c_i, i = 1, 2, \dots 6 \in \mathbb{R} $ and $ d,e,f \in \mathbb{R} $ are known. The unknowns are $x,y,z$. It is assumed that $x,y,z \in [-\pi, \pi) $.

I want to solve this system for the angles $x,y,z$.

My Attempt:

Define the vector of unknowns as

$ X = \begin{bmatrix} x_1 \\ x_2 \\ x_3 \\ x_4 \\ x_5 \\ x_6 \end{bmatrix} = \begin{bmatrix} \cos(x) \\ \sin(x) \\ \cos(y) \\ \sin(y) \\ \cos(z) \\ \sin(z) \end{bmatrix} \tag{4}$

And the matrix

$ A = \begin{bmatrix} a_1 && a_4 && a_2 && a_5 && a_3 && a_6 \\ b_1 && b_4 && b_2 && b_5 && b_3 && b_6 \\ c_1 && c_4 && c_2 && c_5 && c_3 && c_6 \end{bmatrix} \tag{5} $

$ B = \begin{bmatrix} d \\ e \\ f \end{bmatrix} \tag{6}$

Then our system can be written as follows:

$ A X = B \tag{7} $

together with

$ x_1^2 + x_2^2 = 1 \tag{8} $

$ x_3^2 + x_4^2 = 1 \tag{9}$

$ x_5^2 + x_6^2 = 1 \tag{10}$

The first step is to to solve the system $A X = B$. Since a non-homogenous linear system of $3$ equations in $6$ unknowns, the solution will be of the form

$ X = X_0 + C u = H w \tag{11}$

where $X_0 \in \mathbb{R}^6 $ is known , $C \in \mathbb{R}^{6 \times 3 }$ is known, and $ u =[u_1, u_2, u_3]^T \in \mathbb{R}^3 $ is a vector of free parameters. The value of these parameters will be determined from the additional constraints. Defining $w = [u_1, u_2, u_3, 1]^T \in \mathbb{R}^4$, and $H = [ C , X_0] $ gives the above-mentioned equality.

The three given constraints can be written as

$ X^T Q_1 X = 1 $ , $ X^T Q_2 X = 1 $, $ X^T Q_3 X = 1 $

where $Q_1$ has all entries equal to $0$ except the $(1,1)$ and $(2,2)$ entries which are set to $1$.

Similarly $Q_2$ has all entries equal to $0$ except the $(3,3)$ and $(4,4)$ entries which are set to $1$.

And lastly, $Q_3$ has all entries equal to $0$ except the $(5,5)$ and $(6,6)$ entries which are set to $1$.

Since $X = H w $, then we now have the quadratic system of $3$ equations:

$ w^T H^T Q_1 H w = 1 \tag{12a}$

$ w^T H^T Q_2 H w = 1 \tag{12b}$

$ w^T H^T Q_3 H w = 1 \tag{12c}$

$ e_4^T w = 1 \tag{12d}$

where $e_4 = [0,0,0,1]^T $

This system of equations can be solved by a CAS (Computer Algebra System) such as Mathematica, Maple, Sage, etc. There can anywhere from $0$ to $8$ solutions. I did not use any of the online CAS systems. Instead I used a solver for 3 quadratic equations in 3 unknowns that I have developed myself. This problem is addressed in this MSE question and its answer here.

This is a link to the Excel file that contains the source code for the function "intersect_three_quadrics" which solves a system of three quadratic equations in three unknowns, and also other functions that solve a system of two quadratic equations in two unknowns. These are included in this file as macros (VBA script). Click on the link, to open the online file, then click on "Editing" and choose "Open in Desktop App". This will open the file in your desktop Excel program. Click "View" then select "Macros". You can copy the code in this Excel file to a new file and call the solver function "intersect_three_quadrics" from your own VBA code.

Once the unknown $w$ is determined, the $\cos$'s and $\sin$'s of $x,y,z$ vector $X$ can be determined from $X = H w $. And finally the angles $x,y,z$ can be determined using the $\text{Atan2}$ function.

Using my own developed solver, I've written code to test this method, and found that the method works.

An example that I worked on had

$ A = \begin{bmatrix} 16 && 10 && -2.333 && 1.25 && 3.05 && -4.6 \\ 4.05 && 3.9 && -2.2 && 2.6 && 1.15 && 1.95 \\ -3.45 && 5.56 && 6.7 && 4.33 && -5.22 && 13.09 \end{bmatrix} $

and

$ B = \begin{bmatrix} 10 \\ 4 \\ 1 \end{bmatrix} $

The program that I wrote produced $8$ different solutions for $(x,y,z)$, and they are

$ (1.547467576, -2.877830759, -2.985325265) $

$ (1.780814629, 1.976638042, -2.283527333) $

$ (-0.513523632, 0.903408232, 0.321838196) $

$ (-0.160877144, -1.984485011, 1.392156825) $

$ (-0.403278317, 1.857437471, -3.057060696)$

$ (1.572308788, -1.873139221, 0.490617376) $

$ (1.888904049, 1.228551222, -0.598486629) $

$ (-0.133717443, -2.410921989, 2.33919753 ) $

That's all I could come up with. Your comments and alternative solutions are appreciated.

Answer 1

The standard way of numerical solutions is the Newton-Raphson method. $$ a_1\cos x+a_2\cos y +a_3 \cos z +a_4\sin x +a_5\sin y + a_6 \sin z-d=0 ; $$
$$ b_1\cos x+b_2\cos y +b_3 \cos z +b_4\sin x +b_5\sin y + b_6 \sin z-e=0 ; $$
$$ c_1\cos x+c_2\cos y +c_3 \cos z +c_4\sin x +c_5\sin y + c_6 \sin z-f=0 ; $$
The first-order Taylor expansions are $$ a_1\cos (x+\Delta x)+a_2\cos (y+\Delta y) +a_3 \cos (z+\Delta z) +a_4\sin (x+\Delta x) +a_5\sin (y+\Delta y) + a_6 \sin (z+\Delta z)-d $$
$$ \approx a_1\cos x+a_2\cos y +a_3 \cos z +a_4\sin x +a_5\sin y + a_6 \sin z-d $$
$$ -a_1\sin x \Delta x +a_4\cos x \Delta x -a_2\sin y \Delta y +a_5\cos y \Delta y -a_3\sin z \Delta z +a_6\cos z \Delta z $$ plus equivalent expansions for the $b$ and $c$ coefficients. With the usual ansatz of computing the $\Delta x, \Delta y, \Delta z$ given near-misses of zeros for some guessed $x,y,z$ one computes corrections $\Delta x,y,z$ by solving the $3\times 3$ linear system of equations $$ \left( \begin{array}{ccc} -a_1\sin x+a_4\cos x & -a_2\sin y +a_5\cos y & -a_3\sin z + a_6 \cos z\\ -b_1\sin x+b_4\cos x & -b_2\sin y +b_5\cos y & -b_3\sin z + b_6 \cos z\\ -c_1\sin x+c_4\cos x & -c_2\sin y +c_5\cos y & -c_3\sin z + c_6 \cos z \end{array} \right) \cdot \left( \begin{array}{c} \Delta x\\ \Delta y\\ \Delta z \end{array} \right) = $$ $$ \left( \begin{array}{c} -a_1\cos x-a_2\cos y -a_3 \cos z -a_4\sin x -a_5\sin y - a_6 \sin z+d\\ -b_1\cos x-b_2\cos y -b_3 \cos z -b_4\sin x -b_5\sin y - b_6 \sin z+e\\ -c_1\cos x-c_2\cos y -c_3 \cos z -c_4\sin x -c_5\sin y - c_6 \sin z+f\\ \end{array} \right) $$ The following Maple program just verifies that all the 8 solutions are indeed atractors for this iteration:

Digits := 17:
a1 := 16.0 :
a4 := 10.0 :
a2 := -2.333 :
a5 := 1.25 :
a3 := 3.05 :
a6 := -4.6 :
b1 := 4.05 :
b4 := 3.9 :
b2 := -2.2 :
b5 := 2.6 :
b3 := 1.15 :
b6 := 1.95 :
c1 := -3.45 :
c4 := 5.56 :
c2 := 6.7 :
c5 := 4.33 :
c3 := -5.22 :
c6 := 13.09 :
d :=10.0 :
e :=4.0 :
f :=1.0 :
x :=0 ; y:=0 ; z:= 0 ;
x := 1.547 ; y := -2.87 ;  z:= -2.98 ;
x := 1.78 ; y := 1.9 ;  z:= -2.2 ;
x := -0.51 ; y := 0.90 ;  z:= -0.32 ;
x := -0.16 ; y := -1.98 ;  z:= 1.39 ;
x := -0.40 ; y := 1.85 ;  z:= -3.05 ;
x := 1.57 ; y := -1.87 ;  z:= 0.49 ;
x := -0.13 ; y := -2.4 ;  z:= 2.33 ;
for itr from 1 to 10 do
        M := Matrix(3,3) ;
        M[1,1] := -a1*sin(x)+a4*cos(x) ;
        M[1,2] := -a2*sin(y)+a5*cos(y) ;
        M[1,3] := -a3*sin(z)+a6*cos(z) ;
        M[2,1] := -b1*sin(x)+b4*cos(x) ;
        M[2,2] := -b2*sin(y)+b5*cos(y) ;
        M[2,3] := -b3*sin(z)+b6*cos(z) ;
        M[3,1] := -c1*sin(x)+c4*cos(x) ;
        M[3,2] := -c2*sin(y)+c5*cos(y) ;
        M[3,3] := -c3*sin(z)+c6*cos(z) ;
        R := Vector(3) ;
        R[1] := -a1*cos(x)-a4*sin(x) -a2*cos(y)-a5*sin(y) -a3*cos(z)-a6*sin(z)+d ;
        R[2] := -b1*cos(x)-b4*sin(x) -b2*cos(y)-b5*sin(y) -b3*cos(z)-b6*sin(z)+e ;
        R[3] := -c1*cos(x)-c4*sin(x) -c2*cos(y)-c5*sin(y) -c3*cos(z)-c6*sin(z)+f ;
        dd := LinearAlgebra[LinearSolve](M,R) ;
        x := evalf(x+dd[1]) ;
        y := evalf(y+dd[2]) ;
        z := evalf(z+dd[3]) ;
        print(x,y,z) ;
end do: