Search code examples
matlabsymbolic-matheuler-angles

MATLAB solve() can't solve trigonometric matrix for specific values

I'm experiencing something very strange in MATLAB which I can't figure out.

I have 2 sets of coordinates of a body for which I want to find the Euler angles. I set up the rotation matrix R and the symbolic Cardan angles matrix R_cardan to solve for the angles. For the first set I can simply do solve(R==R_cardan) but when I use the second set it doesn't work. The solution returned is empty.

What could be causing this? Here is test code.

clc;clear;close all;

%% For some reason R2 does not want to give a solution
% Find rotation matrix to transform from origin to local reference frame
ex = [ 0.768  0.024 0.640].';
ey = [-0.424  0.768 0.480].';
ez = [-0.480 -0.640 0.600].';

ex2 = [ 0.612372  0.353553 -0.707107].';
ey2 = [0.280330  0.739199 0.612372].';
ez2 = [0.739199 -0.573223 0.353553].';

R = eye(3)*[ex ey ez]
R2 = eye(3)*[ex2 ey2 ez2]
% Symbolic variables
syms beta alpha gamma

% Set up rotatin matrices
R_alpha = [cos(alpha) -sin(alpha) 0; sin(alpha) cos(alpha) 0; 0 0 1]
R_beta = [cos(beta) 0 sin(beta); 0 1 0; -sin(beta) 0 cos(beta)]
R_gamma  = [1 0 0; 0 cos(gamma) -sin(gamma); 0 sin(gamma) cos(gamma)]

% Find symbolic rotation matrix
R_cardan = R_alpha*R_beta*R_gamma

[alpha, beta, gamma] = find_angles(R,R_cardan)
[alpha, beta, gamma] = find_angles(R2,R_cardan) %fails because solution is empty

function [alpha, beta, gamma] = find_angles(R,R_cardan)

% Solve for the angles
sol = solve(R == R_cardan);
alpha = double(sol.alpha(1));
beta = double(sol.beta(1));
gamma = double(sol.gamma(1));

end

My current solution is to just manually calculate the angles which is fine, but I'm interested in what is wrong with my method above.

Solution

  • The problem will be that you solve for an exact match R == R_cardan. As you are working with a finite precision, this is dangerous and it can be that there is no solution, as it happens for R2. Your goal is to find a solution which makes the difference R - R_cardan very small - ideally zero.

    I would do the following instead: Create a loss function

    Loss

    and minimize this function. The ideal solution at R = R_cardan would lead to a loss of zero, but even if this is numerically not possible, you would get a solution which is as close to the optimal solution as possible (in terms of Euclidean distance).

    In MATLAB, this is a bit complicated, but well described in the help pages.

    1. Define the loss function in terms of R and R_cardan, and put all unknowns into a vector x:

      f = sum(sum((R - R_cardan).^2));
x = [alpha; beta; gamma];

    2. Analytically calculate the gradient and Hessian of the loss function:

      gradf = jacobian(f, x).'; % column gradf
hessf = jacobian(gradf, x);

    3. Convert these functions from symbolic functions to MATLAB function handles:

      fh = matlabFunction(f, gradf, hessf, 'vars', {x});

    4. Set up the optimizer to use the gradient and Hessian:

      options = optimoptions('fminunc', 'SpecifyObjectiveGradient', true, ... 
                                  'HessianFcn', 'objective', ...
                                  'Algorithm', 'trust-region')

    5. Minimize!

      solution = fminunc(fh, [0;0;0], options);
alpha_hat = solution(1);
beta_hat = solution(2);
gamma_hat = solution(3);

    For the first example R, this gives the exact same solution as with solve. For the second example R2, the reconstructed matrix R2_hat (obtained by plugging the estimated values into R_cardan) is almost the same as R2, but has some differences in the least significant digits:

    R2 =
    0.6124    0.2803    0.7392
    0.3536    0.7392   -0.5732
   -0.7071    0.6124    0.3536

R2_hat =
    0.6125    0.2805    0.7390
    0.3533    0.7392   -0.5734
   -0.7071    0.6123    0.3537