mirror of https://github.com/ArduPilot/ardupilot
39 lines
939 B
Mathematica
39 lines
939 B
Mathematica
|
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];
|