Improving the smooth path after dual-EKF-navsat node with robot_localization [closed]

edit

Hi, I'm currently using robot_localization package in order to combine data from an IMU, Visual Odometry and GPS. Following the tutorials robot_localization dual-EKF wiki page I'm able to get the correct odometry and tf, But my path is not smooth and i notice a discrete jump on the trajecory. Image example

1. The green path is the Visual Odometry (OrbSlam2)
2. The blue path is the output of ekf_se_map (/odometry/filtered_map )
3. The red is the output of ekf_se_odom
4. The yellow path is the output of navsat_transform_node (/odometry/gps)
5. The white path is the ground truth

this is my tf_tree image tf_tree

I use a rosbag simulation of the kitti dataset.

I'm on ROS melodic, Ubuntu 18.04. Just some general guidelines and things that I could try from improving the smooth path would be greatly appreciated. Thank you in advance!

VO ORBSLAM2 (/odom_cov)

header: 
  seq: 384
  stamp: 
    secs: 1317381172
    nsecs: 210807644
  frame_id: "odom_r"
child_frame_id: "filter_link"
pose: 
  pose: 
    position: 
      x: 255.214271206
      y: -4.10897729772
      z: 30.1642496267
    orientation: 
      x: -0.0498665846097
      y: -0.0129415959665
      z: -0.476836520412
      w: 0.877480923671
  covariance: [0.8, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.8, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.8, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.8, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.8, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.8]
twist: 
  twist: 
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
---

GPS(/gps_cov)

header: 
  seq: 1560
  stamp: 
    secs: 1317381292
    nsecs: 817400695
  frame_id: "gps_link"
status: 
  status: 0
  service: 1
latitude: 48.9721387355
longitude: 8.47601192734
altitude: 198.272659302
position_covariance: [0.5, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.5]
position_covariance_type: 0
---

IMU (/imu_cov)

header: 
  seq: 92
  stamp: 
    secs: 1317381140
    nsecs: 292656139
  frame_id: "i_link"
orientation: 
  x: 0.000643883176529
  y: -0.0433980004141
  z: 0.12066934251
  w: 0.991743469226
orientation_covariance: [0.5, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.5]
angular_velocity: 
  x: 0.00472081300331
  y: -0.0131300385873
  z: 0.0896434253573
angular_velocity_covariance: [0.5, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.5]
linear_acceleration: 
  x: 0.423154205467
  y: 0.855483885359
  z: 9.97805472549
linear_acceleration_covariance: [0.5, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.5]
---

Launcher file ... (more)