《四元数姿态矩阵地磁融合》由会员分享，可在线阅读，更多相关《四元数姿态矩阵地磁融合（14页珍藏版）》请在金锄头文库上搜索。
1、. . . . .发布笔者关于飞行器导航与定位的文章四元数姿态解算中的地磁计融合解读分类：四旋翼飞行器2014-03-23 14:554451人阅读评论(20)收藏举报四元数姿态解算地磁计融合互补滤波融合算法四旋翼飞行器姿态解算姿态融合算法 笔者最近在做四轴，涉及到地磁计的融合算法，网上大多数是x-IMU的融合代码，但是这段代码对于地磁计的融合说明没有做过多的解释，网上没有相关讨论，仅在阿莫论坛看到一篇相关的代码解释，里面有关于地磁计融合部分的解说，个人觉得说的不是很清楚，虽然是正确的，我这里再补充啰嗦一下。 首先给出x-IMU关于陀螺仪、加速度计、地磁计的融合代码：cppview plain
2、copyprint?1. voidMahonyAHRSupdate(floatgx,floatgy,floatgz,floatax,floatay,floataz,floatmx,floatmy,floatmz)2. floatrecipNorm;3. floatq0q0,q0q1,q0q2,q0q3,q1q1,q1q2,q1q3,q2q2,q2q3,q3q3;4. floathx,hy,bx,bz;5. floathalfvx,halfvy,halfvz,halfwx,halfwy,halfwz;6. floathalfex,halfey,halfez;7. floatqa,qb,qc;8.
3、 9. /UseIMUalgorithmifmagnetometermeasurementinvalid(avoidsNaNinmagnetometernormalisation)10. if(mx=0.0f)&(my=0.0f)&(mz=0.0f)11. MahonyAHRSupdateIMU(gx,gy,gz,ax,ay,az); 12. return;13. 14. 15. /Computefeedbackonlyifaccelerometermeasurementvalid(avoidsNaNinaccelerometernormalisation)16. if(!(ax=0.0f)&
4、(ay=0.0f)&(az=0.0f)17. 18. /Normaliseaccelerometermeasurement19. recipNorm=invSqrt(ax*ax+ay*ay+az*az);20. ax*=recipNorm;21. ay*=recipNorm;22. az*=recipNorm;23. 24. /Normalisemagnetometermeasurement25. recipNorm=invSqrt(mx*mx+my*my+mz*mz);26. mx*=recipNorm;27. my*=recipNorm;28. mz*=recipNorm;29. 30.
5、/Auxiliaryvariablestoavoidrepeatedarithmetic31. q0q0=q0*q0;32. q0q1=q0*q1;33. q0q2=q0*q2;34. q0q3=q0*q3;35. q1q1=q1*q1;36. q1q2=q1*q2;37. q1q3=q1*q3;38. q2q2=q2*q2;39. q2q3=q2*q3;40. q3q3=q3*q3;41. 42. /ReferencedirectionofEarthsmagneticfield43. hx=2.0f*(mx*(0.5f-q2q2-q3q3)+my*(q1q2-q0q3)+mz*(q1q3+q
6、0q2);44. hy=2.0f*(mx*(q1q2+q0q3)+my*(0.5f-q1q1-q3q3)+mz*(q2q3-q0q1);45. bx=sqrt(hx*hx+hy*hy);46. bz=2.0f*(mx*(q1q3-q0q2)+my*(q2q3+q0q1)+mz*(0.5f-q1q1-q2q2);47. 48. /Estimateddirectionofgravityandmagneticfield49. halfvx=q1q3-q0q2;50. halfvy=q0q1+q2q3;51. halfvz=q0q0-0.5f+q3q3;52. halfwx=bx*(0.5f-q2q2
7、-q3q3)+bz*(q1q3-q0q2);53. halfwy=bx*(q1q2-q0q3)+bz*(q0q1+q2q3);54. halfwz=bx*(q0q2+q1q3)+bz*(0.5f-q1q1-q2q2);55. 56. /Errorissumofcrossproductbetweenestimateddirectionandmeasureddirectionoffieldvectors57. halfex=(ay*halfvz-az*halfvy)+(my*halfwz-mz*halfwy);58. halfey=(az*halfvx-ax*halfvz)+(mz*halfwx-
8、mx*halfwz);59. halfez=(ax*halfvy-ay*halfvx)+(mx*halfwy-my*halfwx);60. 61. /Computeandapplyintegralfeedbackifenabled62. if(twoKi0.0f)63. integralFBx+=twoKi*halfex*(1.0f/sampleFreq);/integralerrorscaledbyKi64. integralFBy+=twoKi*halfey*(1.0f/sampleFreq);65. integralFBz+=twoKi*halfez*(1.0f/sampleFreq);
9、66. gx+=integralFBx;/applyintegralfeedback67. gy+=integralFBy;68. gz+=integralFBz;69. 70. else71. integralFBx=0.0f;/preventintegralwindup72. integralFBy=0.0f;73. integralFBz=0.0f;74. 75. 76. /Applyproportionalfeedback77. gx+=twoKp*halfex;78. gy+=twoKp*halfey;79. gz+=twoKp*halfez;80. 81. 82. /Integra
10、terateofchangeofquaternion83. gx*=(0.5f*(1.0f/sampleFreq);/pre-multiplycommonfactors84. gy*=(0.5f*(1.0f/sampleFreq);85. gz*=(0.5f*(1.0f/sampleFreq);86. qa=q0;87. qb=q1;88. qc=q2;89. q0+=(-qb*gx-qc*gy-q3*gz);90. q1+=(qa*gx+qc*gz-q3*gy);91. q2+=(qa*gy-qb*gz+q3*gx);92. q3+=(qa*gz+qb*gy-qc*gx);93. 94. /Normalisequaternion95. recipNorm=invSqrt(q0*q0+q1*q1+q2*q2+q3*q3);96. q0*=recipNorm;97. q1*=recipNorm;98. q2*=recipNorm;99. q3*=recipNorm;100. 相信有很多人已经理解了加速度计补偿陀螺仪漂移的原理，这部分代码在x-IMU官网上已经给出，大家可以自行下载(http:/www.x-io.co.uk/ope
