How to properly fuse EKF from robot_localization and AMCL

edit

Hello,

I have been able to build a custom map via SLAM that I would like for my robot to navigate through. I am using an extended kalman filter node from the robot_localization package to fuse measurements from wheel encoders and an IMU. Also, I am fusing amcl with the algorithm because the robot possesses a Lidar sensor. The AMCL algorithm provides estimates for pose data to the ekf and it appears to be working adequately since the pose array converges; however, I have noticed that the ekf algorithm has been giving poor results with regard to localizing within the map. More specifically, it appears that at moments where the robot stops after moving forward or backwards, the ekf underestimates the distance traveled by the robot which causes the laser scans of the map to shift from where they should be in the static map. This can be seen clearly below from two videos that show two different scenarios where:

• Robot is constantly being driven forward and stopped intermediately causing laser scans to drift
• Robot is being driven at a constant velocity causing little laser scan drift

As you may also notice, the position covariance continues to grow without bound as I drive the robot longer, so it may be possible that those problems are related. Below my launch file that initializes my sensors and the ekf will be displayed below:

<launch>

<!-- Initializing teleop node-->
<remap from="vel_cmd" to="mega_one/vel_cmd"/>
<node pkg="devbot_teleop" type="teleop_control.py" name="teleop_node" />

<!-- Initializing nodes that will provide sources of odometry-->
<!-- Launching wheel twist node-->
<remap from="twist" to="mega_one/twist" />
<include file="$(find base_controller_twist_converter)/launch/base_controller_twist.launch">
    <!-- Set custom parameters if needed
    <param name="linear_velocity_stdev" type="double" value="" />
    <param name="angular_velocity_stdev" type="double" value="" />
    -->
</include>    
<!-- Launching imu node-->
<remap from="bno055" to="mega_two/bno055" />
<include file="$(find bno055_imu_ros)/launch/bno055_imu.launch">
    <!-- Set custom parameters if needed
    <param name="linear_velocity_stdev" type="double" value="" />
    <param name="angular_velocity_stdev" type="double" value="" />
    -->
</include>


<!-- Initializing localization parameters-->
<arg name="frame_id"                default="base_link" />
<arg name="odom_frame"              default="odom"/>
<arg name="map_frame"               default="map"/>
<arg name="pose_topic"              default="/amcl_pose"/>
<arg name="imu_topic"               default="/imu/data" />
<arg name="twist_topic"             default="/wheel_twist"/>
<arg name="imu_remove_gravitational_acceleration" default="true" />

<!-- Odometry fusion (EKF), refer to demo launch file in robot_localization for more info -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true" output="screen">

    <param name="frequency" value="30"/>
    <param name="sensor_timeout" value="0.1"/>
    <param name="two_d_mode" value="true"/>

    <param name="map_frame" value="$(arg map_frame)"/>
    <param name="odom_frame" value="$(arg odom_frame)"/>
    <param name="base_link_frame" value="$(arg frame_id)"/>
    <param name="world_frame" value="$(arg odom_frame)"/>

    <param name="transform_time_offset" value="0.0"/>

    <param name="twist0" value="$(arg twist_topic)"/>
    <param name="imu0" value="$(arg imu_topic)"/>
    <param name="pose0" value="$(arg pose_topic)"/>

    <!-- The order of the ...