MATLAB: Solving for rotation matrix: Align vector a with vector b, then rotate around vector b

linear algebranonlinearOptimization ToolboxSymbolic Math Toolbox

Currently working on a quadcopter simulation. I have a desired thrust vector, t =sin(30)cos(45)a1+sin(30)sin(45)a2+cos(30)a3, and desired yaw angle, psi = 45. Because we are working with an under-actuated system, I am trying to solve for the rotation matrix that aligns the vector b3 = [0,0,1] (direction of thrust in the body-fixed frame of reference) with the direction of t. My code is included at the bottom. The problem is that it outputs an empty symbolic struct, with no warnings or errors otherwise:
What I am doing wrong? It is an issue with my theory, my implementation, or both? Thank you!
t = [sind(30) * cosd(45), sind(30) * sind(45), cosd(30)]; %desired thrust vector
psi = 45; %desired yaw angle
b3 = [0;0;1]; %body-fixed unit thrust vector
syms phi theta; %setting up to solve what's left of the rotation matrix
eqn = [[cosd(psi)*cosd(theta)-sind(phi)*sind(psi)*sind(theta), -cosd(phi)*sind(psi), cosd(psi)*sind(theta)+cosd(theta)*sind(phi)*sind(phi);
 cosd(theta)*sind(psi)+cosd(psi)*sind(phi)*sind(theta), cosd(phi)*cosd(psi), sind(psi)*sind(theta)-cosd(theta)*sind(phi)*cosd(psi);
 -cosd(phi)*sind(theta), sind(phi), cosd(phi)*cosd(theta)] * b3 == t/norm(t)]; %equation to be solved (general form of the rotation matrix multiplied by body-fixed thrust aligned with t)
S = solve(eqn,[phi theta]); %solve

Best Answer

Here is an idea
  • Use dot product to calculate angle between vectors
  • Use cross product to calculate normal vector of a plane
You have normal vector and angle. COnstruct new rotation matrix
Related Question