robot_localization: a very simple usecase not working?

asked 2016-07-26 14:46:08 -0500

2284 ●40 ●71 ●78 https://github.com/corot

updated 2016-11-07 08:21:23 -0500

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 ...

(more)

Comments

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

Procópio ( 2016-08-03 11:09:48 -0500 )edit

I am having the same issues, if I figure something out, I'll let you know.

Procópio ( 2016-08-03 11:21:17 -0500 )edit

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

jorge ( 2016-08-08 03:32:32 -0500 )edit

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;

Procópio ( 2016-08-08 04:53:34 -0500 )edit

Does that help EKF localization? Think you will keep publishing an outdated pose in between filter updates...

jorge ( 2016-08-09 01:44:39 -0500 )edit

you are right, that is not helping much. I rolled back from that change.

Procópio ( 2016-08-11 08:55:34 -0500 )edit

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.

bageltz ( 2016-09-08 08:04:23 -0500 )edit

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.

bageltz ( 2016-09-08 08:06:28 -0500 )edit

add a comment

answered 2016-08-28 01:57:06 -0500

Tom Moore

flag of United Kingdom of Great Britain and Northern Ireland

13689 ●74 ●240 ●232 http://github.com/ayrton04

updated 2016-11-22 04:25:08 -0500

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:

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

edit flag offensive delete link

Comments

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.

jorge ( 2016-11-07 07:48:33 -0500 )edit

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?

jorge ( 2016-11-07 09:35:17 -0500 )edit

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.

Tom Moore ( 2016-11-07 09:57:05 -0500 )edit

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!

jorge ( 2016-11-07 10:39:55 -0500 )edit

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?

Tom Moore ( 2016-11-07 11:11:20 -0500 )edit

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.

jorge ( 2016-11-07 11:57:57 -0500 )edit

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...)

jorge ( 2016-11-07 11:59:57 -0500 )edit

Can you post a bag file? Go ahead and record everything, including the tf tree.

Tom Moore ( 2016-11-08 11:23:38 -0500 )edit

see more comments

robot_localization: a very simple usecase not working?

Comments

1 Answer

Comments

Question Tools

Stats

Related questions

robot_localization: a very simple usecase not working? edit

Comments

1 Answer

Comments

Question Tools

Stats

Related questions

robot_localization: a very simple usecase not working?