四元数姿态矩阵地磁融合..docx

发布笔者关于飞行器导航与定位的文章四元数姿态解算中的地磁计融合解读分类：?四旋翼飞行器2014-03-23 14:55?4451人阅读?评论(20)?收藏?举报四元数姿态解算地磁计融合互补滤波融合算法四旋翼飞行器姿态解算姿态融合算法 笔者最近在做四轴，涉及到地磁计的融合算法，网上大多数是x-IMU的融合代码，但是这段代码对于地磁计的融合说明没有做过多的解释，网上没有相关讨论，仅在阿莫论坛看到一篇相关的代码解释，里面有关于地磁计融合部分的解说，个人觉得说的不是很清楚，虽然是正确的，我这里再补充啰嗦一下。?首先给出x-IMU关于陀螺仪、加速度计、地磁计的融合代码：[cpp]?view plaincopyprint?/snippets/251884/forkvoid?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);?? ?return;???}????//?Compute?feedback?only?if?accelerometer?measurement?valid?(avoids?NaN?in?accelerometer?normalisation)??if(!((ax?==?0.0f)??(ay?==?0.0f)??(az?==?0.0f)))?{????//?Normalise?accelerometer?measurement??recipNorm?=?invSqrt(ax?*?ax?+?ay?*?ay?+?az?*?az);???ax?*=?recipNorm;???ay?*=?recipNorm;???az?*=?recipNorm;??//?Normalise?magnetometer?measurement??recipNorm?=?invSqrt(mx?*?mx?+?my?*?my?+?mz?*?mz);???mx?*=?recipNorm;???my?*=?recipNorm;???mz?*=?recipNorm;??//?Auxiliary?variables?to?avoid?repeated?arithmetic??q0q0?=?q0?*?q0;???q0q1?=?q0?*?q1;???q0q2?=?q0?*?q2;???q0q3?=?q0?*?q3;???q1q1?=?q1?*?q1;???q1q2?=?q1?*?q2;???q1q3?=?q1?*?q3;???q2q2?=?q2?*?q2;???q2q3?=?q2?*?q3;???q3q3?=?q3?*?q3;??//?Reference?direction?of?Earths?magnetic?field??hx?=?2.0f?*?(mx?*?(0.5f?-?q2q2?-?q3q3)?+?my?*?(q1q2?-?q0q3)?+?mz?*?(q1q3?+?q0q2));???hy?=?2.0f?*?(mx?*?(q1q2?+?q0q3)?+?my?*?(0.5f?-?q1q1?-?q3q3)?+?mz?*?(q2q3?-?q0q1));???bx?=?sqrt(hx?*?hx?+?hy?*?hy);???bz?=?2.0f?*?(mx?*?(q1q3?-?q0q2)?+?my?*?(q2q3?+?q0q1)?+?mz?*?(0.5f?-?q1q1?-?q2q2));????//?Estimated?direction?of?gravity?and?magnetic?field??halfvx?=?q1q3?-?q0q2;???halfvy?=?q0q1?+?q2q3;??