problem configuring robot localization

edit

Hello,

I am trying to use the robot localization package into my underwater robot. at the moment I am doing some dry tests on the system to make sure everything is working properly.

I have my parameter Yaml file as following:

frequency: 30
sensor_timeout: 0.1
two_d_mode: false
print_diagnostics: true #echo the /diagnostics_agg topic for details


## IMU 0 data from the pixhawk
imu0: /mavros/imu/data
odom_frame: odom_fil

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                 #
imu0_twist_rejection_threshold: 0.8                #
imu0_linear_acceleration_rejection_threshold: 0.8  #
imu0_remove_gravitational_acceleration: false

# odom from the visual odometry
odom0: /stereo_odometer/odometry
odom0_config: [true, true, true,
               true, true, true,
               false, false, false,
               false, false, false,
               false, false, false]
odom0_differential: false
odom0_relative: true
odom0_queue_size: 5
odom0_pose_rejection_threshold: 2
odom0_twist_rejection_threshold: 0.2
odom0_nodelay: false

and I have the launch file to load thees parameters as following:

<launch>
  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se" clear_params="true">
    <rosparam command="load" file="$(find ghattas_localization)/params/ekf.yaml" />
  </node>
</launch>

at the moment the Imu is giving correct values on the topic /mavros/imu/data and the camera is giving correct visual odometry on the topic /stereo_odometer/odometry

the problem arise when I fuse those two topic information into the robot localization package here is a sample of the visual odometry and the imu data I have after moving the sensors together along the three axis back and forward:

IMU

---
header: 
  seq: 73756
  stamp: 
    secs: 1554683660
    nsecs:  25350080
  frame_id: "base_link"
orientation: 
  x: 0.690330453259
  y: -0.259261357207
  z: 0.203541852432
  w: -0.644048234429
orientation_covariance: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
angular_velocity: 
  x: 0.0266555175185
  y: 0.0747747048736
  z: 0.0135522447526
angular_velocity_covariance: [1.2184696791468346e-07, 0.0, 0.0, 0.0, 1.2184696791468346e-07, 0.0, 0.0, 0.0, 1.2184696791468346e-07]
linear_acceleration: 
  x: -0.5491724
  y: -9.8458766
  z: -0.9610517
linear_acceleration_covariance: [8.999999999999999e-08, 0.0, 0.0, 0.0, 8.999999999999999e-08, 0.0, 0.0, 0.0, 8.999999999999999e-08]
---

visual odometry

header: 
  seq: 1165
  stamp: 
    secs: 0
    nsecs:         0
  frame_id: "/odom"
child_frame_id: "zed_camera"
pose: 
  pose: 
    position: 
      x: -0.0270988403684
      y: 0.533030086882
      z: -0.381215711715
    orientation: 
      x: 0.0170582143098
      y: 0.178282180763
      z: 0.0473209938349
      w: 0.982692833437
  covariance: [0.1, 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.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.17, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.17, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.17]
twist: 
  twist: 
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.002, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0 ...