Fuse /odom with IMU from RealSense d435i

edit

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

<node pkg="robot_localization" type="ekf_localization_node" name="ekf_odom_node" output="screen" &gt;<="" p="">

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