ROS imu_filter_madgwick 源码分析

  主要对阅读imu_filter_madgwick这个功能包的主要源码提供了阅读的思路，适用于原功能包。

  功能包在ros_wiki上可以找到。

 

<node pkg="imu_filter_madgwick" type="imu_filter_node" name="imu_filter_madgwick" output="screen" respawn="false" >

        <param name="fixed_frame" value="base_footprint" />

        <param name="use_mag" value="fasle" />

        <param name="publish_tf" value="false" />

        <param name="use_magnetic_field_msg" value="fasle" />

        <param name="world_frame" value="enu" />

        <param name="orientation_stddev" value="0.05" />

        <param name="angular_scale" value="1.0" />

</node>

一．

Src 下的 imu_filter_ros.cpp

117行订阅imu_calib 功能包发布的imu/data话题

146行imu_subscriber_->registerCallback(&ImuFilterRos::imuCallback, this);

161行void ImuFilterRos::imuCallback(const ImuMsg::ConstPtr& imu_msg_raw)

208行 filter_.madgwickAHRSupdateIMU

在功能包imu_filter.cpp中

258行 ImuFilter::madgwickAHRSupdateIMU

268行 orientationChangeFromGyro (q0, q1, q2, q3, gx, gy, gz, qDot1, qDot2, qDot3, qDot4) 四元数在陀螺仪上的变化率  得到qDot1  qDot2  qDot3  qDot4

291行 因为<param name="world_frame" value="enu" /> 所以运行这个switch 语句

116 行 static inline void addGradientDescentStep  得到 s0 s1 s2 s3

         这个函数得到了的是角速度

只有当加速度计测量有效时才计算反馈Gradient decent algorithm corrective step梯度体面算法校正步骤

295行 归一化

311行normalizeQuaternion (q0, q1, q2, q3);

在imu_filter_ros.cpp

212行 publishFilteredMsg(imu_msg_raw);

330行 void ImuFilterRos::publishFilteredMsg(const ImuMsg::ConstPtr& imu_msg_raw)

356行 imu_publisher_.publish(imu_msg)

            发布的内容包括339-354的内容

在Imu_filter下

72行  精确规范化