九轴姿态算法x_9轴融合算法,9轴算法资源.docxVIP

?首先给出x-IMU关于陀螺仪、加速度计、地磁计的融合代码：

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);??

????????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;