N3_sub/libraries/AP_NavEKF/Models/QuaternionMathExample/calcQ.m

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]);