mirror of https://github.com/ArduPilot/ardupilot
46 lines
2.3 KiB
Mathematica
46 lines
2.3 KiB
Mathematica
|
function Q = calcQ(daxNoise,dayNoise,dazNoise,dvxNoise,dvyNoise,dvzNoise,q0,q1,q2,q3)
|
||
|
|
%CALCQ
|
||
|
|
% Q = CALCQ(DAXNOISE,DAYNOISE,DAZNOISE,DVXNOISE,DVYNOISE,DVZNOISE,Q0,Q1,Q2,Q3)
|
||
|
|
|
||
|
|
% This function was generated by the Symbolic Math Toolbox version 5.8.
|
||
|
|
% 14-Jan-2015 11:09:07
|
||
|
|
|
||
|
|
t2 = dayNoise.*q2.*q3.*(1.0./4.0);
|
||
|
|
t3 = t2-daxNoise.*q0.*q1.*(1.0./4.0)-dazNoise.*q2.*q3.*(1.0./4.0);
|
||
|
|
t4 = q3.^2;
|
||
|
|
t5 = q2.^2;
|
||
|
|
t6 = dazNoise.*q1.*q3.*(1.0./4.0);
|
||
|
|
t7 = t6-daxNoise.*q1.*q3.*(1.0./4.0)-dayNoise.*q0.*q2.*(1.0./4.0);
|
||
|
|
t8 = daxNoise.*q0.*q3.*(1.0./4.0);
|
||
|
|
t9 = t8-dayNoise.*q0.*q3.*(1.0./4.0)-dazNoise.*q1.*q2.*(1.0./4.0);
|
||
|
|
t10 = q0.^2;
|
||
|
|
t11 = q1.^2;
|
||
|
|
t12 = daxNoise.*q1.*q2.*(1.0./4.0);
|
||
|
|
t13 = t12-dayNoise.*q1.*q2.*(1.0./4.0)-dazNoise.*q0.*q3.*(1.0./4.0);
|
||
|
|
t14 = dazNoise.*q0.*q2.*(1.0./4.0);
|
||
|
|
t15 = t14-daxNoise.*q0.*q2.*(1.0./4.0)-dayNoise.*q1.*q3.*(1.0./4.0);
|
||
|
|
t16 = dayNoise.*q0.*q1.*(1.0./4.0);
|
||
|
|
t17 = t16-daxNoise.*q2.*q3.*(1.0./4.0)-dazNoise.*q0.*q1.*(1.0./4.0);
|
||
|
|
t21 = q0.*q3.*2.0;
|
||
|
|
t22 = q1.*q2.*2.0;
|
||
|
|
t18 = t21-t22;
|
||
|
|
t23 = q0.*q2.*2.0;
|
||
|
|
t24 = q1.*q3.*2.0;
|
||
|
|
t19 = t23+t24;
|
||
|
|
t20 = t4+t5-t10-t11;
|
||
|
|
t25 = q0.*q1.*2.0;
|
||
|
|
t26 = t21+t22;
|
||
|
|
t32 = t4-t5-t10+t11;
|
||
|
|
t27 = dvyNoise.*t18.*t32;
|
||
|
|
t28 = q2.*q3.*2.0;
|
||
|
|
t29 = t25-t28;
|
||
|
|
t30 = t4-t5-t10+t11;
|
||
|
|
t31 = t25+t28;
|
||
|
|
t33 = t4-t5+t10-t11;
|
||
|
|
t34 = t23-t24;
|
||
|
|
t35 = dvxNoise.*t34.*(t4+t5-t10-t11);
|
||
|
|
t36 = dvzNoise.*t19.*t33;
|
||
|
|
t37 = t35+t36-dvyNoise.*t18.*t31;
|
||
|
|
t38 = -dvxNoise.*t26.*t34-dvyNoise.*t31.*t32-dvzNoise.*t29.*t33;
|
||
|
|
Q = reshape([daxNoise.*t11.*(1.0./4.0)+dayNoise.*t5.*(1.0./4.0)+dazNoise.*t4.*(1.0./4.0),t3,t7,t13,0.0,0.0,0.0,0.0,0.0,0.0,t3,daxNoise.*t10.*(1.0./4.0)+dayNoise.*t4.*(1.0./4.0)+dazNoise.*t5.*(1.0./4.0),t9,t15,0.0,0.0,0.0,0.0,0.0,0.0,t7,t9,daxNoise.*t4.*(1.0./4.0)+dayNoise.*t10.*(1.0./4.0)+dazNoise.*t11.*(1.0./4.0),t17,0.0,0.0,0.0,0.0,0.0,0.0,t13,t15,t17,daxNoise.*t5.*(1.0./4.0)+dayNoise.*t11.*(1.0./4.0)+dazNoise.*t10.*(1.0./4.0),0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,dvxNoise.*t20.^2+dvyNoise.*t18.^2+dvzNoise.*t19.^2,t27-dvxNoise.*t20.*t26-dvzNoise.*t19.*t29,t37,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t27-dvzNoise.*t19.*(t25-q2.*q3.*2.0)-dvxNoise.*t20.*t26,dvxNoise.*t26.^2+dvyNoise.*t30.^2+dvzNoise.*t29.^2,t38,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t37,t38,dvxNoise.*t34.^2+dvyNoise.*t31.^2+dvzNoise.*t33.^2,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0],[10, 10]);
|