0
$\begingroup$

Rosanswers logo

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 tried to debug the problem in many ways. First I checked both inputs of the robot localization package. I echoed the radar velocity and made sense with what I commanded (around 1 m/s in x velocity). I also tried just fusing the radar velocity in the robot localization filter. Obviously you have no way to sense that you are turning so the odometry/filtered topic shows that the estimated trajectory is just a straight but CONTINUOUS line. This leads me to believe that the radar is not causing the jumps and something is wrong with the IMU.

Then I visualized the IMU topic as a box in rviz (with IMU plugin) and it matched with the trajectory described above. I also checked that the IMU was following REP-103 and that a counter-clockwise turn corresponds to an increase in the yaw angle.

The transforms that I provide are very simple, I just say that the radar and IMU are exactly the same as the base_link frame. I am assuming Clearpath built the IMU in the center of the Jackal. Also, the radar measures body linear velocities so it is not critical if the radar has an offset.

I played around with the yaml file parameters but nothing changed for the better.

I also changed the covariance of the IMU's orientation but still no big difference.

This image shows the exact moment where the estimated position blows up: image description

This one shows when the velocity blows up: image description


I hope I provided enough information and let me know if someone needs me to provide any more files or information.

Thanks sooo much, and I would really appreciate if anybody could take a look at this and give some help.


Problem solved

Since the orientation comes from filtering and integrating accel and gyro, adding those to the filter made it blow up. So fusing just orientation solved the problem.

imu0_config: [false, false, false,
                  true,  true,  true,
                  false, false, false,
                  false, false, false,
                  false, false, false]

It also helped not fusing y-velocity in the radar since it is a wheeled robot and cannot move sideways (but that is for a more accurate result)

twist0_config: [false, false, false,
                false, false, false,
                true,  false,  true,
                false, false, false,
                false, false, false]

Resulting odom in rviz:

image description


Originally posted by xaru8145 on ROS Answers with karma: 105 on 2020-03-06

Post score: 0


Original comments

Comment by gvdhoorn on 2020-03-08:
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.

Comment by xaru8145 on 2020-03-09:
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.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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


Originally posted by stevemacenski with karma: 8272 on 2020-03-09

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by stevemacenski on 2020-03-09:
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.

Comment by xaru8145 on 2020-03-09:
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!

$\endgroup$

Your Answer

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