ardupilot/libraries/AP_NavEKF/Models/QuaternionMathExample/calcF.m

37 lines
1.3 KiB
Matlab

function F = calcF(dax,dax_b,day,day_b,daz,daz_b,dvx,dvy,dvz,q0,q1,q2,q3)
%CALCF
% F = CALCF(DAX,DAX_B,DAY,DAY_B,DAZ,DAZ_B,DVX,DVY,DVZ,Q0,Q1,Q2,Q3)
% This function was generated by the Symbolic Math Toolbox version 5.8.
% 14-Jan-2015 11:09:08
t2 = dax_b.*(1.0./2.0);
t3 = daz_b.*(1.0./2.0);
t4 = day_b.*(1.0./2.0);
t8 = day.*(1.0./2.0);
t5 = t4-t8;
t6 = q3.*(1.0./2.0);
t7 = q2.*(1.0./2.0);
t9 = daz.*(1.0./2.0);
t10 = dax.*(1.0./2.0);
t11 = -t2+t10;
t12 = q1.*(1.0./2.0);
t13 = -t3+t9;
t14 = -t4+t8;
t15 = dvz.*q1.*2.0;
t16 = dvy.*q1.*2.0;
t17 = dvz.*q0.*2.0;
t18 = dvx.*q1.*2.0;
t19 = dvy.*q2.*2.0;
t20 = dvz.*q3.*2.0;
t21 = t18+t19+t20;
t22 = dvx.*q0.*2.0;
t23 = dvz.*q2.*2.0;
t29 = dvy.*q3.*2.0;
t24 = t22+t23-t29;
t25 = dvx.*q2.*2.0;
t26 = dvx.*q3.*2.0;
t27 = dvy.*q0.*2.0;
t28 = -t15+t26+t27;
F = reshape([1.0,t11,t14,t13,t24,t28,t16+t17-t25,0.0,0.0,0.0,dax.*(-1.0./2.0)+t2,1.0,t3-t9,t14,t21,-t16-t17+t25,t28,0.0,0.0,0.0,t5,t13,1.0,t2-t10,t16+t17-dvx.*q2.*2.0,t21,-t22-t23+t29,0.0,0.0,0.0,daz.*(-1.0./2.0)+t3,t5,t11,1.0,t15-dvx.*q3.*2.0-dvy.*q0.*2.0,t24,t21,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,t12,q0.*(-1.0./2.0),-t6,t7,0.0,0.0,0.0,1.0,0.0,0.0,t7,t6,q0.*(-1.0./2.0),-t12,0.0,0.0,0.0,0.0,1.0,0.0,t6,-t7,t12,q0.*(-1.0./2.0),0.0,0.0,0.0,0.0,0.0,1.0],[10, 10]);