ardupilot/libraries/AP_NavEKF/Models/GimbalEstimatorExample/calcTms.m

15 lines
434 B
Mathematica
Raw Normal View History

function Tms = calcTms(gPhi,gPsi,gTheta)
%CALCTMS
% TMS = CALCTMS(GPHI,GPSI,GTHETA)
% This function was generated by the Symbolic Math Toolbox version 6.1.
% 15-Feb-2015 16:02:09
t2 = cos(gTheta);
t3 = sin(gPsi);
t4 = cos(gPsi);
t5 = sin(gPhi);
t6 = sin(gTheta);
t7 = cos(gPhi);
Tms = reshape([t2.*t4-t3.*t5.*t6,-t3.*t7,t4.*t6+t2.*t3.*t5,t2.*t3+t4.*t5.*t6,t4.*t7,t3.*t6-t2.*t4.*t5,-t6.*t7,t5,t2.*t7],[3, 3]);