IMU without compas-Kinematic Only Fusion Issues

edit

Hello dear community,

Im trying to fuse IMU+kinematic model fusion with robot_localization package. My IMU sensor only measures lateral and longitudinal acceleration and yaw rate which also does not give covariances of measurements. On the other hand, I have 4 wheel encoder data which is used for kinematic bicycle model.

GOAL : The fusion of kinematic model's x-y position and velocity information with IMU yaw rate,x-y acceleration to achieve accurate lateral and longitudunal velocity, lateral acceleration,yaw rate in the car's COG direction.

PROBLEM : * IMU does not really contribute to lateral velocity while longitudunal velocity, lateral accelaration and yaw rate fusions are okay. So lateral velocity and yaw angle of odometry output is terrible. After a while when the robot is going straight lateral velocity is bigger more than 5m/s.*

This is how I filled empty covariance data of IMU message :

data2.orientation_covariance = (1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9)

data2.angular_velocity_covariance = (1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-5)

data2.linear_acceleration_covariance = (1e-4, 1e-9, 1e-9, 1e-9, 0.5, 1e-9, 1e-9, 1e-9, 1e-9)

When I'm trying to fuse them in robot_localization package, IMU does not contribute to lateral velocity. And drift in IMU message causes really wrong fused lateral velocity . Also I observed two major problems in the "debug.txt" file generated by the package :

1. Timestamp is sometimes same , so package says "delta is 0" in some correction steps like below,

   Measurement time is 1455209554.6450054646, last measurement time is 1455209554.6450054646, delta is 0
   

2. Second is package states message is enqueued.

   3 measurements in queue.
   

I really don't know if it is a problem, because I guess the package uses older messages to predict next state which causes inaccurate estimations.

In my config file,

<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">

  <param name="transform_time_offset" value="0.0001"/>
  <param name="transform_timeout" value="0"/>
  <param name="predict_to_current_time" value="true"/>
  <param name="publish_acceleration" 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"/>

  <!-- odom0 ve imu0 config axes order -->
  <!-- X Y Z -->
  <!-- R P Y -->
  <!-- X' Y' Z' -->
  <!-- R' P' Y' -->
  <!-- X'' Y'' Z'' -->
  <param name="odom0" value="/Kinematic_Odom"/>
  <rosparam param="odom0_config">[true,  true,  false,
                                  false,  false,  true,
                                  true,   true,  false,
                                  false,  false,  false,
                                  false,  false,  false]</rosparam>

  <param name="imu0" value="/imu_fixed"/>
  <rosparam param="imu0_config">[false,  false,  false,
                                 false,  false,  false,
                                 false,  false,  false,
                                 false,  false,  true,
                                 true,   true,  false]</rosparam>



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

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

  <param name="debug" value="true"/>
  <param name="debug_out_file" value="/home/furkan/Desktop/debug.txt"/>

  <!-- config axes order -->
  <!-- X Y Z -->
  <!-- R P Y -->
  <!-- X' Y' Z' -->
  < ...