3D pose with IMU and odometry using robot localization

edit

Hi all!

My objective is simple, yet I haven't been able to successfully tackle it. I have a small robot, it is a 4wd robot, 4 motors, 4 encoders, but it behaves like a differential robot actually. Odometry from encoders is working well (except when one of the wheels gets stuck in the terrain and starts to slip...). I'm using ROS Melodic on ubuntu 18.04.

You can see my robot here:

I want to fuse odom, with an IMU and later with gps in order to get a nice 3d pose, not just 2d.

So, I started with odom + imu (as suggested in all r_l guides) in 2d. I actually have 2 imus: * imu0 -> swift piksi multi via ethernet to my robot computer at 25hz * imu2 -> mpu 6050 at 25hz (flight controller naze32)

The axis of the IMUs align physically with the robot odometry, x forward... and bla bla, as showed in this image:

This is a sensor sample for the odom, and both imus:

Of course, I follow the instructions to check gravity and the signs. My ekf launch files looks like this:

<launch>
  <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" />

    <param name="frequency" value="20." />  
    <param name="sensor_timeout" value="0.2" />  
    <param name="publish_tf" value="true" />
    <!-- <param name="map_frame" value="map" /> -->
    <param name="odom_frame" value="odom" />
    <param name="base_link_frame" value="base_link" />
    <param name="world_frame" value="odom" />
    <param name="print_diagnostics" value="true" />
    <param name="debug" value="true" />

    <param name="use_control" value="false" />
    <param name="two_d_mode" value="true" />

    <param name="odom0" value="odom_front"/>
    <param name="imu0" value="/imu2/data"/> 

    <rosparam param="odom0_config">[true,  true,  false, 
                                  false, false, true, 
                                  true, true, false, 
                                  false, false, true,
                                  false, false, false]</rosparam>                                                                   

    <rosparam param="imu0_config">[false, false, false, 
                                 true, true, true,
                                 true, true, true, 
                                 true, true, true,
                                 true, true, true]</rosparam>

    <param name="odom0_differential" value="true"/> 
    <param name="imu0_differential" value="true"/>

    <param name="odom0_queue_size" value="10"/>
    <param name="imu0_queue_size" value="10"/>

   <param name="imu0_remove_gravitational_acceleration" value="true" /> 

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