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

edit

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  #

• Bagfile: sensors4RL.bagI could not attach the bag file in the post, so I added it to a drive folder: https://drive.google.com/drive/folder...

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:

Recorded topics:

• /mmWaveDataHdl/velocity: velocity output of the radar

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

Debugging

I have ... (more)