mirror of https://github.com/ArduPilot/ardupilot
37 lines
1.3 KiB
Mathematica
37 lines
1.3 KiB
Mathematica
|
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]);
|