1 GPS, 2 IMUs and 1 Odom: Filtered odom value jumps when GPS updates

edit

Hi, I'm trying to feed one GPS, two IMU and one odom signals to localize the vehicle. The problem is, the output odom is jumping when GPS value updates.

Here's the flowchart of how I fed the signals:

Observation: The frequency of IMU1, IMU2, raw_odom and GPS are: 100 Hz, 50 Hz, 50 Hz and 1 Hz. Obviously, the GPS signal is subject to discrete jumps.

First, the output estimate position (/odometry/filtered_odom) can continuously moves. When GPS signal updates, the output position just jumps to the updated GPS position (if the current output estimate position has offsets with the new GPS ones). The jump happens every second as GPS's frequency is 1 Hz.

Ideal Outcome: Discard jumping, making the output smooth.

Possible Issue: I guess it might caused by wrong frame_id, but right now I'm still not 100% clear in understanding tf.

Thanks for helping me out! I can provide any other data if necessary,

ROS bag File:https://drive.google.com/file/d/16QCF...

Launch file:

<launch>
<node name="map_bl_tf" pkg="tf" type="static_transform_publisher" respawn="true" args="0 0 0 0 0 0 1 map odom 100" output="log"/>
<node name="map_odom_bl" pkg="tf" type="static_transform_publisher" respawn="true" args="0 0 0 0 0 0 1 odom base_link 100" output="log"/>

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

    <!-- 
    Use this when fully the params are fully tuned:
    <rosparam command="load" file="$(find vehicle_state_detection)/src/params/ekf_odom_1.yaml" />
    -->

    <param name="frequency" value="30" />
    <param name="sensor_timeout" value="0.1" />
    <param name="two_d_mode" 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="map" />

    <param name="transform_time_offset" value="0.0" />
    <param name="transform_timeout" value="0.0" />

    <!--
      imu0: IMU from gnss 
      imu1: IMU from vehicle through CAN
      odom0: odom from vehicle though wheel_encoder and twist info
      odom1: odom from the output of navsat_transformation_node
  -->
    <param name="imu0" value="/mti/sensor/imu"/>
    <param name="imu1" value="/vehicle/imu/data_raw"/>
    <param name="odom0" value="/vehicle/odom/data_raw"/>
    <param name="odom1" value="/odometry/filtered_odom_vehicle"/>

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

    <param name="imu0_differential" value="true"/>
    <param name="imu1_differential" value="true"/>
    <param name="odom0_differential" value="false"/>
    <param name="odom1_differential" value="false"/>

    <param name="imu0_relative" value="false"/>
    <param name="imu1_relative" value="false"/>
    <param name="odom0_relative" value="false"/>
    <param name="odom1_relative" value="false"/>

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

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

    <param name="imu0_queue_size" value="10"/>
    <param name="imu1_queue_size" value="10"/>
    <param name="odom0_queue_size" value="10"/>
    <param name="odom1_queue_size ...