Home Explore Help
Register Sign In
wangdan
/
watchrobot
1
0
Fork 0
Files Issues 0 Pull Requests 0 Wiki
Branch: master
Branches Tags
watchrobot
/
libraries
/
AP_NavEKF
/
Models
/
GimbalEstimatorExample
/
calcH_MAG.m

calcH_MAG.m 2.1 KB
Permalink History Raw

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980 
  1. function H_MAG = calcH_MAG(gPhi,gPsi,gTheta,magX,magY,magZ,q0,q1,q2,q3)
    2. 

  2. %CALCH_MAG
    3. 

  3. %    H_MAG = CALCH_MAG(GPHI,GPSI,GTHETA,MAGX,MAGY,MAGZ,Q0,Q1,Q2,Q3)
    4. 

  4. 

  5. %    This function was generated by the Symbolic Math Toolbox version 6.1.
    6. 

  6. %    15-Feb-2015 16:00:09
    7. 

  7. 

  8. t2 = cos(gPhi);
    9. 

  9. t3 = cos(gTheta);
    10. 

  10. t4 = sin(gPhi);
    11. 

  11. t5 = sin(gTheta);
    12. 

  12. t6 = q0.^2;
    13. 

  13. t7 = q1.^2;
    14. 

  14. t8 = q2.^2;
    15. 

  15. t9 = q3.^2;
    16. 

  16. t10 = t6+t7-t8-t9;
    17. 

  17. t11 = sin(gPsi);
    18. 

  18. t12 = cos(gPsi);
    19. 

  19. t13 = q0.*q2.*2.0;
    20. 

  20. t14 = q1.*q3.*2.0;
    21. 

  21. t15 = t13+t14;
    22. 

  22. t16 = q0.*q3.*2.0;
    23. 

  23. t18 = q1.*q2.*2.0;
    24. 

  24. t17 = t16-t18;
    25. 

  25. t19 = t3.*t11;
    26. 

  26. t20 = t4.*t5.*t12;
    27. 

  27. t21 = t19+t20;
    28. 

  28. t22 = t16+t18;
    29. 

  29. t23 = t5.*t11;
    30. 

  30. t41 = t3.*t4.*t12;
    31. 

  31. t24 = t23-t41;
    32. 

  32. t25 = q0.*q1.*2.0;
    33. 

  33. t31 = q2.*q3.*2.0;
    34. 

  34. t26 = t25-t31;
    35. 

  35. t27 = t6-t7+t8-t9;
    36. 

  36. t28 = t5.*t12;
    37. 

  37. t29 = t3.*t4.*t11;
    38. 

  38. t30 = t28+t29;
    39. 

  39. t32 = t3.*t12;
    40. 

  40. t46 = t4.*t5.*t11;
    41. 

  41. t33 = t32-t46;
    42. 

  42. t35 = t4.*t17;
    43. 

  43. t36 = t2.*t5.*t10;
    44. 

  44. t37 = t2.*t3.*t15;
    45. 

  45. t38 = t35+t36-t37;
    46. 

  46. t39 = magZ.*t38;
    47. 

  47. t40 = t10.*t21;
    48. 

  48. t42 = t15.*t24;
    49. 

  49. t43 = t2.*t12.*t17;
    50. 

  50. t44 = t40+t42-t43;
    51. 

  51. t45 = magY.*t44;
    52. 

  52. t47 = t10.*t33;
    53. 

  53. t48 = t15.*t30;
    54. 

  54. t49 = t2.*t11.*t17;
    55. 

  55. t50 = t47+t48+t49;
    56. 

  56. t51 = magX.*t50;
    57. 

  57. t52 = -t39+t45+t51;
    58. 

  58. t53 = 1.0./t52;
    59. 

  59. t54 = t4.*t27;
    60. 

  60. t55 = t2.*t3.*t26;
    61. 

  61. t56 = t2.*t5.*t22;
    62. 

  62. t57 = -t54+t55+t56;
    63. 

  63. t58 = magZ.*t57;
    64. 

  64. t59 = t21.*t22;
    65. 

  65. t60 = t24.*t26;
    66. 

  66. t61 = t2.*t12.*t27;
    67. 

  67. t62 = t59-t60+t61;
    68. 

  68. t63 = magY.*t62;
    69. 

  69. t64 = t26.*t30;
    70. 

  70. t65 = t22.*t33;
    71. 

  71. t66 = t2.*t11.*t27;
    72. 

  72. t67 = t64-t65+t66;
    73. 

  73. t68 = magX.*t67;
    74. 

  74. t69 = t58-t63+t68;
    75. 

  75. t70 = t53.*t69;
    76. 

  76. t34 = tan(t70);
    77. 

  77. t71 = t34.^2;
    78. 

  78. t72 = t71+1.0;
    79. 

  79. t73 = 1.0./t52.^2;
    80. 

  80. H_MAG = [-t72.*(t53.*(magZ.*(t4.*t26+t2.*t3.*t27)+magY.*(t24.*t27+t2.*t12.*t26)+magX.*(t27.*t30-t2.*t11.*t26))-t69.*t73.*(magZ.*(t4.*t15+t2.*t3.*t17)+magY.*(t17.*t24+t2.*t12.*t15)+magX.*(t17.*t30-t2.*t11.*t15))),t72.*(t53.*(magZ.*(t2.*t3.*t22-t2.*t5.*t26)+magY.*(t22.*t24+t21.*t26)+magX.*(t22.*t30+t26.*t33))+t69.*t73.*(magZ.*(t2.*t3.*t10+t2.*t5.*t15)+magY.*(t10.*t24-t15.*t21)+magX.*(t10.*t30-t15.*t33))),t72.*(t53.*(-magZ.*(t4.*t22+t2.*t5.*t27)+magY.*(t21.*t27-t2.*t12.*t22)+magX.*(t27.*t33+t2.*t11.*t22))-t69.*t73.*(magZ.*(t4.*t10-t2.*t5.*t17)+magY.*(t17.*t21+t2.*t10.*t12)+magX.*(t17.*t33-t2.*t10.*t11))),0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0];
    81.