Fuse /odom with IMU from RealSense d435i
Hello,
I have a problem with my 4 wheeled differential driver robot. I have used equation for two wheeled odometry computation and it work good for linear but not for angular. The angular part is very bad, I have almost 90 degree offset on one full turn. So I have boat the RealSense d435i cam and I want to fuse the odometry with IMU.
I have tried some params for robot_localization but the odometry didn't change too mach, it still very bad regarding rotation part.
Maybe someone had an experience working with D435i IMU and already know the good params to fuse is with wheel odometry.
Another strange behavior of the camera IMU is that it is not coincided with camera link. So it the IMU frame from imufiltermadgwick is like upside-down. And I don't know hot to make it coincident with cam of odom, to have the same axes. I have attach an image:
Thanks very much in advance!
EDIT 1
<param name="frequency" value="30"/>
<param name="sensor_timeout" value="0.2"/>
<param name="two_d_mode" value="false"/>
<remap from="odometry/filtered" to="/odom/ekf"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_footprint"/>
<param name="world_frame" value="odom"/>
<param name="transform_time_offset" value="0.1"/>
<!-- ODOM 1 -->
<param name="odom0" value="/gnom/driver_1/odom"/>
<param name="odom0_differential" value="false" />
<param name="odom0_relative" value="false" />
<param name="odom0_queue_size" value="10" />
<rosparam param="odom0_config">[true, true, false,
false, false, true,
true, true, false,
false, false, true,
false, false, false]</rosparam>
<!-- ODOM 2 -->
<param name="odom0" value="/gnom/driver_2/odom"/>
<param name="odom0_differential" value="true" />
<param name="odom0_relative" value="false" />
<param name="odom0_queue_size" value="10" />
<rosparam param="odom0_config">[false, false, false,
false, false, false,
true, true, false,
false, false, true,
false, false, false]</rosparam>
<!-- IMU -->
<!--param name="imu0" value="/gnom/imu/data"/>
<param name="imu0_differential" value="false" />
<param name="imu0_relative" value="true" />
<param name="imu0_queue_size" value="10" />
<param name="imu0_remove_gravitational_acceleration" value="true" />
<rosparam param="imu0_config">[false, false, false,
false, false, false,
false, false, false,
false , false, true,
false, false, false]</rosparam-->
<!-- GENERAL -->
<param name="print_diagnostics" value="true" />
<param name="debug" value="false" />
<param name="debug_out_file" value="debug_odom_ekf.txt" />
<rosparam param="process_noise_covariance">[0.05, 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.06, 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.03, 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.025,0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.04, 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, 0, 0, 0, 0, 0.01, 0, 0, 0, 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, 0.01, 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, 0, 0, 0, 0, 0.005]</rosparam>
<rosparam param="initial_estimate_covariance">[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, 1 , 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1 , 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, 1, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 , 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 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>
I have added my params for robot_localization! As a reference I have taken almost everything from this tutorial: https://roverrobotics.com/blogs/guides/fusing-imu-encoders-with-ros-robot-localization
Asked by Yehor on 2021-07-15 06:47:11 UTC
Comments
Could you show us the settings of the robot localization and how you calculate the covariances? Because the problem with robot_localization usually is highly connected with wrongs values of covariances.
Asked by Teo Cardoso on 2021-07-15 08:20:43 UTC
@Teo Cardoso I made EDIT 1 with params
Asked by Yehor on 2021-07-15 08:24:58 UTC
Try to set the imu0_config with angular velocity x,y, and z to true. And tell me if this changes something. And activate the two_d_mode
Asked by Teo Cardoso on 2021-07-15 09:06:06 UTC
@Teo Cardoso I will try now. I also confused about the axes of the IMU frame. It is upside-down, can it cause the problem?
Asked by Yehor on 2021-07-15 09:13:54 UTC
@Teo Cardoso I have tried with that params for IMU:
The base footprint frame rotates very smooth, but it is still error 90 degree on one full rotation
Asked by Yehor on 2021-07-15 09:19:07 UTC
@Yehor, I have a similar setup using D435i, but i can't get the IMU working with the wheel encoders even for linear part, can you share the setup ?
Asked by crispylel on 2021-08-04 13:49:26 UTC