四元数姿态矩阵地磁融合论述.docxVIP

发布笔者关于飞行器导航与定位的文章  HYPERLINK /nemol1990/article/details四元数姿态解算中的地磁计融合解读 分类：? HYPERLINK /yzhajlydy/article/category/1764467 四旋翼飞行器2014-03-23 14:55?4451人阅读? HYPERLINK /nemol1990/article/detailsutm_source=tuicool \l comments 评论(20)? HYPERLINK javascript:void(0); \o 收藏 收藏? HYPERLINK /nemol1990/article/detailsutm_source=tuicool \l report \o 举报 举报  HYPERLINK /tag/%e5%9b%9b%e5%85%83%e6%95%b0%e5%a7%bf%e6%80%81%e8%a7%a3%e7%ae%97 \t _blank 四元数姿态解算 HYPERLINK /tag/%e5%9c%b0%e7%a3%81%e8%ae%a1%e8%9e%8d%e5%90%88 \t _blank 地磁计融合 HYPERLINK /tag/%e4%ba%92%e8%a1%a5%e6%bb%a4%e6%b3%a2%e8%9e%8d%e5%90%88%e7%ae%97%e6%b3%95 \t _blank 互补滤波融合算法 HYPERLINK /tag/%e5%9b%9b%e6%97%8b%e7%bf%bc%e9%a3%9e%e8%a1%8c%e5%99%a8%e5%a7%bf%e6%80%81%e8%a7%a3%e7%ae%97 \t _blank 四旋翼飞行器姿态解算 HYPERLINK /tag/%e5%a7%bf%e6%80%81%e8%9e%8d%e5%90%88%e7%ae%97%e6%b3%95 \t _blank 姿态融合算法 ? ? 笔者最近在做四轴，涉及到地磁计的融合算法，网上大多数是x-IMU的融合代码，但是这段代码对于地磁计的融合说明没有做过多的解释，网上没有相关讨论，仅在阿莫论坛看到一篇相关的代码解释，里面有关于地磁计融合部分的解说，个人觉得说的不是很清楚，虽然是正确的，我这里再补充啰嗦一下。 ? ??首先给出x-IMU关于陀螺仪、加速度计、地磁计的融合代码： [cpp]? HYPERLINK /nemol1990/article/detailsutm_source=tuicool \o view plain view plain HYPERLINK /nemol1990/article/detailsutm_source=tuicool \o copy copy HYPERLINK /nemol1990/article/detailsutm_source=tuicool \o print print HYPERLINK /nemol1990/article/detailsutm_source=tuicool \o ? ? void?MahonyAHRSupdate(float?gx,?float?gy,?float?gz,?float?ax,?float?ay,?float?az,?float?mx,?float?my,?float?mz)?{?? ????float?recipNorm;?? ????float?q0q0,?q0q1,?q0q2,?q0q3,?q1q1,?q1q2,?q1q3,?q2q2,?q2q3,?q3q3;???? ????float?hx,?hy,?bx,?bz;?? ????float?halfvx,?halfvy,?halfvz,?halfwx,?halfwy,?halfwz;?? ????float?halfex,?halfey,?halfez;?? ????float?qa,?qb,?qc;?? ?? ????//?Use?IMU?algorithm?if?magnetometer?measurement?invalid?(avoids?NaN?in?magnetometer?normalisation)?? ????if((mx?==?0.0f)??(my?==?0.0f)??(mz?==?0.0f))?{?? ????????MahonyAHRSupdateIMU(gx,?gy,?gz,?ax,?ay,?az);?? ???????