0
$\begingroup$

Rosanswers logo

Hi, I'm trying to use robot_localization to fuse two localization sources: amcl + feature-based localization. But just starting with a very simple case, namely filtering amcl input without any fusion, works very bad for the rotation. It's clear to me that either I'm misunderstanding how robot_localization works, or I did a very stupid mistake on the configuration. Here's my launch file:

<launch>
    <node ns="absolute" pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true" respawn="true">

      <!-- ======== STANDARD PARAMETERS ======== -->
      <param name="frequency" value="30"/>
      <param name="sensor_timeout" value="0.5"/>
      <param name="two_d_mode" value="true"/>

      <param name="map_frame" value="map"/>
      <param name="odom_frame" value="odom"/>
      <param name="base_link_frame" value="base_footprint"/>
      <param name="world_frame" value="map"/>

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

      <param name="pose0" value="/amcl_pose"/> 

      <!-- AMCL estimates X, Y and yaw against a known map, so use those absolute measures -->
      <rosparam param="pose0_config">[true,  true,  false,
                                      false, false, true,
                                      false, false, false,
                                      false, false, false,
                                      false, false, false]</rosparam>

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

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

      <param name="print_diagnostics" value="true"/>

      <!-- ======== ADVANCED PARAMETERS ======== -->

      <param name="pose0_queue_size" value="1"/>

      <rosparam param="process_noise_covariance">[0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                  0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.00, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015]</rosparam>

           <rosparam param="initial_estimate_covariance">[1e2,  0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    1e2,  0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,   10,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-9,  0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-9,  0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1e-9,  0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1e-9, 0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-9, 0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]</rosparam>


      <!--  Placeholder for output topic remapping
      <remap from="odometry/filtered" to=""/>
      -->
      <remap from="set_pose" to="/initialpose" />
    </node>
</launch>

Both in simulation (stage) and with a real robot the odom frame rotates a lot relative to map when the robot turns:

example using stage simulation

I can greatly mitigate this by including the odometry in the filter, but to my understanding just filter the amcl posed should provide a global pose (map -> odom tf) at least as good as a standalone amcl. In fact, my intention is to use robot_localization same way I use now amcl, to correct the drift of the odometry. But in most robot_localization configurations I have seen, people combine both global and local localization sources.

What am I doing wrong? Am I completely misguided? Thank you a lot

Btw, I use indigo on ubuntu 14.04

UPDATE

I cloned and compiled the indigo-devel branch. Starting with the ekf_template.yaml example configuration, I tried to run my minimal configuration of amcl pose and odometry to provide the robot pose: ekf_localization_node provides map -> odom tf based on odometry prediction and amcl poses correction, while the robot base provides odom -> base_link. My configuration (one of many attempts):

modified ekf_template.yaml

With all the possible changes that came to my mind trying pose coordinates, velocity, relative, differential, 2D, 3D with no success. Even in Stage simulation, I'm unable to provide an stable pose estimation:

another example using stage simulation, running code from github

Thank you for any feedback or ideally working configuration.

UPDATE 2

I managed to improve things by lowering the yaw process noise to 0.01 and raising A LOT the odom covariance (1.0!!!). But still robot rotations pollute costmaps continuously. In this capture, I increase rotational speed from 0.1 to 1.0 rad/s. The faster robot spins, the worse the yaw provided by ekf_localization_node.

I wonder if there's a way to passthrough the yaw as provided by amcl + odom (very stable) and use the EKF filter just to merge X and Y. Any suggestion?


Originally posted by jorge on ROS Answers with karma: 2284 on 2016-07-26

Post score: 1


Original comments

Comment by Procópio on 2016-08-03:
have you turned off the amcl tf_broadcast (Set this to false to prevent amcl from publishing the transform between the global frame and the odometry frame)?

Comment by Procópio on 2016-08-03:
I am having the same issues, if I figure something out, I'll let you know.

Comment by jorge on 2016-08-08:
Hi, yes, duplicated tf is not the problem. In fact, I'm getting better results tweaking the process_noise_covariance and increasing the input odom messages covariance

Comment by Procópio on 2016-08-08:
thanks for the feeback. I am tweaking the amcl code now, I noticed that it was publishing at a very low rate and I am forcing it to publish the pose every loop now, even if amcl the filter has not updated. at amcl_node.cpp line 1095, I have this now: bool force_publication = true;

Comment by jorge on 2016-08-09:
Does that help EKF localization? Think you will keep publishing an outdated pose in between filter updates...

Comment by Procópio on 2016-08-11:
you are right, that is not helping much. I rolled back from that change.

Comment by bageltz on 2016-09-08:
Hi I also tested the node with a single absolute pose source first. However, if I set pose0_differential and pose0_relative to false, I couldn't get any output. I can get output while pose0_differential is true, and it starts from 0, which is not I wanted.

Comment by bageltz on 2016-09-08:
I also tried your launch file, and had the same problem. Do you have any suggestion to get the node outputting filtered absolute values? Thanks.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

So one thing to change right off the bat is that if you are using pose data from amcl and you need the filter to more or less immediately converge to that same pose, then you need to increase the process_noise_covariance for X, Y, and yaw. Otherwise, the filter will trust its own estimate more than the measurement, and since there's no velocity being measured, that's going to be sluggish. Also, note that amcl does not constantly update its pose. It only sends out pose updates when it reaches the thresholds that are defined in its configuration. You should have some velocity references being fused as well.

EDIT 1:

I still haven't had a chance to look at your bag (sorry, getting set up and reviewing someone's bagged data takes a considerable amount of time that I can't afford right now), but just looking at the EKF config that you linked, I can see some problems right off the bat:

  1. I'm referring to this block:

    odom0: /odom
    odom0_config: [true,true,false,
                   false, false, false,
                   false, false, false,
                   false, false, true,
                   false, false, false]
    odom0_differential: false
    odom0_relative: false
    odom0_queue_size: 2
    odom0_pose_rejection_threshold: 5
    odom0_twist_rejection_threshold: 1
    odom0_nodelay: false
    
    pose0: /amcl_pose
    pose0_config: [true,  true,  false,
                   false, false, false,
                   false, false, false,
                   false, false, false,
                   false, false, false]
    pose0_differential: true
    pose0_relative: false
    pose0_queue_size: 5
    pose0_rejection_threshold: 2
    pose0_nodelay: false
    

First, you are fusing absolute pose variables from two different sources. Don't do that. If you drive forward 10 meters and your amcl pose puts you at (10, 0) and your wheel odometry puts you at (9.1, 0), then the filter is going to rapidly jump back and forth between those two readings. Change your wheel odometry config to only fuse velocities.

Second, get rid of the rejection thresholds. Those are parameters that should not be included until you've had a change to solidly characterize the noise in your measurements.

Third, enable nodelay for the odometry data. The odometry message size is above a threshold that causes trouble with Nagle's algorithm, causing messages to arrive at strange intervals.

  1. The EKF is, obviously, a filter. All filters will induce a non-zero delay in output. If your amcl data is fused in an EKF, it's going to have at least a small amount of lag. You can minimize this, as I said, by increasing the process_noise_covariance for a given variable. I realize you've done this.

  2. Can you repeat your rotation test by eliminating the second EKF (the one fusing the amcl data) and let amcl publish the map->odom transform?


Originally posted by Tom Moore with karma: 13689 on 2016-08-28

This answer was ACCEPTED on the original site

Post score: 2


Original comments

Comment by jorge on 2016-11-07:
Thanks @Tom. Actually, X and Y are fine. My problems come from rotation (check my capture in UPDATE 2). I managed to improve things by lowering the yaw process noise to 0.01 and raising A LOT the odom covariance (1.0!!!). But still robot rotations pollute costmaps due to mismatching laser scan.

Comment by jorge on 2016-11-07:
I wonder if the noise in the yaw comes from the fact that the filter uses the same map -> odom tf it creates to transform amcl pose from base_link to odom (substracting odom -> base_link tf). This can trigger positive feedback loops that could explain the noise. Makes this sense?

Comment by Tom Moore on 2016-11-07:
Just checking: you did turn off the broadcast of map->odom transform by amcl, correct? Also, if the amcl pose is reported in the map frame and you are feeding it into an EKF with its world_frame set to map, then no transformation occurs.

Comment by jorge on 2016-11-07:
yes, tf_broadcast is disabled. But... EKF tracks map -> base_link, but it broadcasts map -> odom. So I suppose at some point there must be a odom ->base_link subtraction. But well, it's just a guess,,, I never inspected the code carefully!

Comment by Tom Moore on 2016-11-07:
Yes, it absolutely uses the odom->base_link transform, you are correct. But you are fusing the pose data from amcl, which, if I recall, is NOT given in the base_footprint frame, but in the map frame. Can you add a sample amcl message?

Comment by jorge on 2016-11-07:
Yes, sure, amcl poses are on map frame. But in EKF it gets combined with the odometry. I tried to make it work without odometry and didn't work at all; robot tf spins like crazy while the robot is not moving at all.

Comment by jorge on 2016-11-07:
To your knowledge, would it make sense to combine ONLY global poses (amcl and other landmark-based global poses) without any input for the prediction phase? (odom, IMU, cmd_vel...)

Comment by Tom Moore on 2016-11-08:
Can you post a bag file? Go ahead and record everything, including the tf tree.

Comment by jorge on 2016-11-09:
The wiki don't let me attach files,,, so here is a dropbox link. Can you open it? I recorded something similar to what I showed in the animated GIF, accelerating from 0 to 1 rad/s and the stopping sharply. Thanks!

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.