资源描述：
《四元数姿态矩阵地磁融合.doc》由会员上传分享，免费在线阅读，更多相关内容在行业资料-天天文库。
1、.发布笔者关于飞行器导航与定位的文章四元数姿态解算中的地磁计融合解读分类： 四旋翼飞行器2014-03-2314:55 4451人阅读 评论(20) 收藏 举报四元数姿态解算地磁计融合互补滤波融合算法四旋翼飞行器姿态解算姿态融合算法 笔者最近在做四轴，涉及到地磁计的融合算法，网上大多数是x-IMU的融合代码，但是这段代码对于地磁计的融合说明没有做过多的解释，网上没有相关讨论，仅在阿莫论坛看到一篇相关的代码解释，里面有关于地磁计融合部分的解说，个人觉得说的不是很清楚，虽然是正确的，我这里再补充啰嗦一下。 首先给出x-IMU关于陀螺仪、
2、加速度计、地磁计的融合代码：[cpp] viewplaincopyprint?1.void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { 2. float recipNorm; 3. float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; 4. float hx, h
3、y, bx, bz; 5. float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz; 6. float halfex, halfey, halfez; 7. float qa, qb, qc; 8. 9. // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation) 10. if((mx == 0.0f) &&
4、(my == 0.0f) && (mz == 0.0f)) { 11. MahonyAHRSupdateIMU(gx, gy, gz, ax, ay, az); 12. return; 13. } 14. 15. // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) 16. if(!((ax == 0.0f) && (ay ==
5、0.0f) && (az == 0.0f))) { 17. 18. // Normalise accelerometer measurement 19. recipNorm = invSqrt(ax * ax + ay * ay + az * az); 20. ax *= recipNorm; 21. ay *= recipNorm; 22. az *= recipNorm; 23. 24. // Normalise magnet
6、ometer measurement 25. recipNorm = invSqrt(mx * mx + my * my + mz * mz); word范文.1. mx *= recipNorm; 2. my *= recipNorm; 3. mz *= recipNorm; 4. 5. // Auxiliary variables to avoid repeated arithmetic 6. q0q0 = q0 * q0; 7.
7、 q0q1 = q0 * q1; 8. q0q2 = q0 * q2; 9. q0q3 = q0 * q3; 10. q1q1 = q1 * q1; 11. q1q2 = q1 * q2; 12. q1q3 = q1 * q3; 13. q2q2 = q2 * q2; 14. q2q3 = q2 * q3; 15. q3q3 = q3 * q3; 16. 17. // Ref
8、erence direction of Earth's magnetic field 18. hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) +
