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

Position and linear velocity blow up when using Robot Localization (IMU+Radar)

asked 2020-03-06 13:19:04 -0600

xaru8145 gravatar image

updated 2022-05-23 09:52:07 -0600

lucasw gravatar image

Hello,

I am trying to estimate odometry from IMU and radar sensors using robot localization. The filter starts good after a few seconds but then both the position (x,y) and linear velocity (x,y) blow up to really high values. I am using a Jackal from Clearpath Robotics and I have my own NUC with Ubuntu 18.04 and ros-melodic connected through Ethernet to the onboard computer of the Jackal.


Sensors:

  • Since I am using the Jackal, I use their built-in IMU filtered through the imu_filter_madgwick. It spits out 0 values for the orientation covariance so I just created a cpp file that rewrites the 0's in the diagonals for a specific value: 1e-5. The filtered IMU with added covariance is published to the topic: /imu/data_added_cov

  • For the Radar, I use the TI AWR1843BOOST and I interface it with the google ROS package: https://github.com/cstahoviak/goggles


Files:

The launch file:

<launch>
 <node pkg="tf2_ros" type="static_transform_publisher" name="tf_imu" args="0.0 0.0 0.0 0.0 0.0 0 1.0 base_link imu_link"/>
 <node pkg="tf2_ros" type="static_transform_publisher" name="tf_radar" args="0.0 0.0 0.0 0.0 0.0 0 1.0 base_link base_radar_link"/>
 <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se" clear_params="true">
        <rosparam command="load" file="$(find robot_localization)/params/ekf_template.yaml"/>
 </node>
</launch>

Params file: ekf_template.yaml

frequency: 30
sensor_timeout: 0.1
two_d_mode: true
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
debug_out_file: /path/to/debug/file.txt
publish_tf: true
publish_acceleration: false

map_frame: map              # Defaults to "map" if unspecified
odom_frame: odom            # Defaults to "odom" if unspecified
base_link_frame: base_link  # Defaults to "base_link" if unspecified
world_frame: odom           # Defaults to the value of odom_frame if unspecified

twist0: /mmWaveDataHdl/velocity
twist0_config: [false, false, false,
                false, false, false,
                true,  true,  true,
                false, false, false,
                false, false, false]
twist0_queue_size: 3
#twist0_rejection_threshold: 2
twist0_nodelay: false

# Jackal's IMU is in ENU frame so it conforms to REP-103
imu0: /imu/data_added_cov
imu0_config: [false, false, false,
              true,  true,  true,
              false, false, false,
              true,  true,  true,
              true,  true,  true]
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 5
#imu0_pose_rejection_threshold: 0.8                 # Note the difference in parameter names
#imu0_twist_rejection_threshold: 0.8                #
#imu0_linear_acceleration_rejection_threshold: 0.8  #

Test description:

The test is recorded in sensors4RL.bag bag file. I basically start driving on my lab and get out to a straight corridor. The trajectory is a rectangle from that point so there 3 main right hand turns after I get out of the lab (after 30 seconds in the bag file). I am controlling the robot using a remote bluetooth PS3 controller. The forward speed that I am commanding is 1 m/s in the straight parts of the trajectory.

Trajectory:

image description

Recorded topics:

  • /mmWaveDataHdl/velocity: velocity output of the radar

  • /imu/data_added_cov: Imu filtered topic with added covariance


Debugging

I have ... (more)

edit retag flag offensive close merge delete

Comments

Please attach your screenshots directly to your post. I've given you sufficient karma.

The launch file, yaml (both from robot localization package) file

Please include at least the robot_localization configuration in your post here. Your Google drive link will disappear, reducing this posts value significantly.

Also:

created a shortened version Bagfile 2 (2 GB).

It would probably be good if you could prune that bag file more. I doubt many ppl will want to download 2GB just to reproduce your problem.

gvdhoorn gravatar image gvdhoorn  ( 2020-03-08 06:35:43 -0600 )edit

Thanks a lot for the feedback, it is the first question I have posted here so I was not sure how to do it. The bagfile (sensors4RL.bag) has been downsized to 6.9MB and now it only includes the sensor data needed to fuse in robot localization (radar and IMU). I have also included the launch and params file in the post and added the images. One should only access the google drive link to download the bag file.

xaru8145 gravatar image xaru8145  ( 2020-03-09 09:47:30 -0600 )edit

1 Answer

Sort by » oldest newest most voted
1

answered 2020-03-09 09:56:50 -0600

On a top level:

The position covariance will always blow up when you have no position information being inputted. Neither of your inputs is being processed with XYZ information.

Your IMU is double counting RPY and their derivatives. See the RL configuration recommendations. Fuse only RPY if you have them. And you’re fusing accelerations. Remove the accelerations unless you have a very, very nice IMU that is well calibrated. That is a likely candidate source of your issues.

Show me an example IMU and Radar message. Make sure both have proper covariance values for the fused elements and that they’re realistic (your IMU one sounds fine, but you didn’t specify the radar).

edit flag offensive delete link more

Comments

As an aside- how do you like the radar? I had the chance to play with one for awhile and the aliasing issues while the radar was in motion mounted on my robot made it irritating at best to use in trying to detect dynamic obstacles. Computing odometry from it sounds interesting though.

stevemacenski gravatar image stevemacenski  ( 2020-03-09 09:58:38 -0600 )edit

Thanks a lot Steve, that totally worked! I read that recommendation before but I interpreted that accelerations would not cause any problem. I think I tried fusing IMU's orientation and accel but still had the same issue. Now removed both and only fused orientation and worked great!

As for the radar, with velocity I do not have any issues. I am not sure about the depthcloud but I mainly use the radar in static environments.

I edited the question and added the visualization of the estimated odometry by RL using IMU and radar. Thanks so much again!

xaru8145 gravatar image xaru8145  ( 2020-03-09 11:20:51 -0600 )edit

Question Tools

2 followers

Stats

Asked: 2020-03-06 13:19:04 -0600

Seen: 545 times

Last updated: Mar 09 '20