ardupilot/libraries/AP_NavEKF/Models/AttErrVecMathExample/calcH_MAG.m

39 lines
939 B
Matlab

function H_MAG = calcH_MAG(magX,magY,magZ,q0,q1,q2,q3)
%CALCH_MAG
% H_MAG = CALCH_MAG(MAGX,MAGY,MAGZ,Q0,Q1,Q2,Q3)
% This function was generated by the Symbolic Math Toolbox version 5.8.
% 27-Dec-2014 13:59:09
t2 = q0.^2;
t3 = q1.^2;
t4 = q2.^2;
t5 = q3.^2;
t6 = q0.*q2.*2.0;
t7 = q1.*q3.*2.0;
t8 = t6+t7;
t9 = q0.*q3.*2.0;
t13 = q1.*q2.*2.0;
t10 = t9-t13;
t11 = t2+t3-t4-t5;
t12 = magX.*t11;
t14 = magZ.*t8;
t19 = magY.*t10;
t15 = t12+t14-t19;
t16 = t2-t3+t4-t5;
t17 = q0.*q1.*2.0;
t24 = q2.*q3.*2.0;
t18 = t17-t24;
t20 = 1.0./t15;
t21 = magY.*t16;
t22 = t9+t13;
t23 = magX.*t22;
t28 = magZ.*t18;
t25 = t21+t23-t28;
t29 = t20.*t25;
t26 = tan(t29);
t27 = 1.0./t15.^2;
t30 = t26.^2;
t31 = t30+1.0;
H_MAG = [-t31.*(t20.*(magZ.*t16+magY.*t18)+t25.*t27.*(magY.*t8+magZ.*t10)),t31.*(t20.*(magX.*t18+magZ.*t22)+t25.*t27.*(magX.*t8-magZ.*t11)),t31.*(t20.*(magX.*t16-magY.*t22)+t25.*t27.*(magX.*t10+magY.*t11)),0.0,0.0,0.0,0.0,0.0,0.0];