ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

How to properly fuse EKF from robot_localization and AMCL

asked 2023-03-30 02:11:18 -0500

Devin1126 gravatar image


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:

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:


<!-- Initializing teleop node-->
<remap from="vel_cmd" to="mega_one/vel_cmd"/>
<node pkg="devbot_teleop" type="" 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="" />
<!-- 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="" />

<!-- 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 ...
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted

answered 2023-04-03 14:47:55 -0500

Devin1126 gravatar image

I have actual found the solution to my laser scan shifting problem. The issue was not related to the way that I configured the ekf_localization_node. It actually was related to the odom_alpha parameters of the amcl node that I was launching separately. If your odometry source is not very precise, then modifying the odom_alpha parameters to make them larger in the amcl node can compensate for that in localizing the robot within the map. More details can be found here.

edit flag offensive delete link more

Question Tools

1 follower


Asked: 2023-03-30 02:11:18 -0500

Seen: 669 times

Last updated: Apr 03 '23