Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

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

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 link of flowchart of how I fed the signals: https://drive.google.com/file/d/1ZgXKLhqVFdprLXiR9m0XGbrB0QESbUtO/view?usp=sharing

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,

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" value="20"/>

    <param name="debug" value="false"/>
    <param name="debug_out_file" value="debug_ekf_localization.txt"/>

    <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.01, 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.1,  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,    0,    0,    0,    0,    0.04, 0,    0,    0,    0,    0,    0,    
              0,    0,    0,    0,    0,    0,    0,    0,    0,    0.01, 0,    0,    0,    0,    0,    
              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0.01, 0,    0,    0,    0,    
              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0.01, 0,    0,    0,    
              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0.01, 0,    0,    
              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0.01, 0,    
              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0.01  ]</rosparam>

    <rosparam param="initial_estimate_covariance">
            [1e-6, 0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    
             0,    1e-6, 0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    
             0,    0,    1e-6, 0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    
             0,    0,    0,    1e-6, 0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    
             0,    0,    0,    0,    1e-6, 0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    
             0,    0,    0,    0,    0,    1e-6, 0,    0,    0,    0,    0,    0,    0,    0,    0,    
             0,    0,    0,    0,    0,    0,    1e-6, 0,    0,    0,    0,    0,    0,    0,    0,    
             0,    0,    0,    0,    0,    0,    0,    1e-6, 0,    0,    0,    0,    0,    0,    0,    
             0,    0,    0,    0,    0,    0,    0,    0,    1e-6, 0,    0,    0,    0,    0,    0,    
             0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-6, 0,    0,    0,    0,    0,    
             0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-6, 0,    0,    0,    0,    
             0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-6, 0,    0,    0,    
             0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-6, 0,    0,    
             0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-6, 0,    
             0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-6  ]</rosparam>

    <remap from="/odometry/filtered" to="/odometry/filtered_odom" />
</node>


<node pkg="robot_localization" type="navsat_transform_node" name="vehicle_navsat_transform_node" clear_params="true">

    <!-- Placeholders for input remapping. Set your topic names as the "to" values.
    <remap from="imu/data" to=""/>
    <remap from="odometry/filtered" to=""/>
    <remap from="gps/fix" to=""/>
    -->
    <param name="frequency" value="30" />
    <param name="delay" value="0" />
    <param name="magnetic_declination_radians" value="-0.249625367" />
    <param name="yaw_offset" value="0.22" />
    <param name="zero_altitude" value="true" />
    <param name="broadcast_utm_transform" value="false" />
    <param name="publish_filtered_gps" value="false" />
    <param name="use_odometry_yaw" value="true" />
    <param name="wait_for_datum" value="true" />
    <rosparam param="datum">[42.275921, -71.798370, 0.0]</rosparam>
    <!--
      input: 
           1. fused odom data from ekf_odom_1: 
            /odometry/filtered -> /odometry/filtered_odom
           2. raw gps data from vehicle's GPS driver:
              /gps/fix -> /vehicle/gps/fix2
      -->
    <remap from="/odometry/filtered" to="/odometry/filtered_odom" />
    <remap from="/gps/fix" to="/vehicle/gps/fix2" />
    <!--
      output:
           1. fused GPS message:
              /gps/filtered -> /gps/filtered_odom_vehicle
           2. fused odom message: (#FIXME: looks wrong, therefore not be used!)
          /odometry/gps -> /odometry/filtered_odom_vehicle
      -->
    <remap from="/odometry/gps" to="/odometry/filtered_odom_vehicle" />
    <remap from="/gps/filtered" to="/gps/filtered_odom_vehicle" />

</node>
</launch>

Sample Data:

From IMU1: (rostopic: /vehicle/imu/data_raw)

header: 
  seq: 312698
  stamp: 
    secs: 1521833157
    nsecs: 194273627
  frame_id: "base_footprint"
orientation: 
  x: 0.0
  y: 0.0
  z: 0.0
  w: 0.0
orientation_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: 0.0004
  y: 0.0
  z: 0.005
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration: 
  x: -0.18
  y: 0.02
  z: -9.82
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

From IMU2: (rostopic: /mti/sensor/imu)

header: 
  seq: 1126
  stamp: 
    secs: 3583
    nsecs: 537100000
  frame_id: "mti/data"
orientation: 
  x: 0.0
  y: 0.0
  z: 0.0
  w: 0.0
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: -0.00785939209163
  y: -0.0112495776266
  z: 0.015035610646
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration: 
  x: -0.409770280123
  y: -0.113449186087
  z: 9.92660903931
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

raw Odom from wheel encoder: (rostopic: /vehicle/odom/data_raw)

header: 
  seq: 3510
  stamp: 
    secs: 1523511193
    nsecs: 254081010
  frame_id: "odom"
child_frame_id: "base_link"
pose: 
  pose: 
    position: 
      x: 25.0594859288
      y: -32.5312823615
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.390051939181
      w: 0.920792856587
  covariance: [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, 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]
twist: 
  twist: 
    linear: 
      x: 2.51111102104
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.346135645213
  covariance: [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, 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]

From GPS_driver: (rostopic: /vehicle/gps/fix2)

header: 
  seq: 21
  stamp: 
    secs: 1523511141
    nsecs: 589173078
  frame_id: "base_link"
status: 
  status: 0
  service: 0
latitude: 42.2756033333
longitude: -71.7981716667
altitude: 152.25
position_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
position_covariance_type: 0

Output from navsat_transform node: (rostopic: /odometry/filtered_odom_vehicle)

header: 
  seq: 2442
  stamp: 
    secs: 1523511203
    nsecs:   5774975
  frame_id: "map"
child_frame_id: "base_link"
pose: 
  pose: 
    position: 
      x: 21.0497827582
      y: -2.52271801714
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.792082581899
      w: 0.610413944347
  covariance: [0.04228150575870658, 0.006547072346641349, 0.0, 0.0, 0.0, -0.029262488820821206, 0.006547072346641346, 0.018975901890026334, 0.0, 0.0, 0.0, -0.007661683197784052, 0.0, 0.0, 4.987548663770124e-07, 1.4502935722807452e-19, -6.941216635616791e-14, 0.0, 0.0, 0.0, 1.4502935722807271e-19, 4.995835707030885e-07, 8.085457769023848e-24, 0.0, 0.0, 0.0, -6.941216635616789e-14, 8.085457746617754e-24, 4.99583570699219e-07, 0.0, -0.0292624888208212, -0.0076616831977840485, 0.0, 0.0, 0.0, 0.044752510509221695]
twist: 
  twist: 
    linear: 
      x: 3.34638040916
      y: -0.000663673175027
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0427353974701
  covariance: [0.029937610018694723, -1.748788884986028e-06, 0.0, 0.0, 0.0, -1.9526198758763966e-06, -1.7487888849861702e-06, 0.031781089816912814, 0.0, 0.0, 0.0, -0.00029328346609445733, 0.0, 0.0, 4.996875475492092e-07, -4.146782564862857e-23, 1.0922949666341516e-23, 0.0, 0.0, 0.0, -4.146782564862858e-23, 4.987548512708783e-07, 2.3359823844380062e-27, 0.0, 0.0, 0.0, 1.0922949666341511e-23, 2.335982408268371e-27, 4.987548512708783e-07, 0.0, -1.9526198758802387e-06, -0.0002932834660944581, 0.0, 0.0, 0.0, 0.035568126302575054]

Output filtered estimation: (rostopic: /odometry/filtered_odom)

header: 
  seq: 405
  stamp: 
    secs: 1523512494
    nsecs: 659452915
  frame_id: "map"
child_frame_id: "base_link"
pose: 
  pose: 
    position: 
      x: 4.3553063759
      y: -30.6477541417
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: -0.506579466828
      w: 0.862193275193
  covariance: [0.04513444652848379, 0.0094500678928774, 0.0, 0.0, 0.0, 0.025429218630340738, 0.009450067892877393, 0.03012814700393296, 0.0, 0.0, 0.0, 0.012462089384510879, 0.0, 0.0, 4.987328908167554e-07, -2.290360084568565e-16, -4.7911713210145933e-14, 0.0, 0.0, 0.0, -2.2903600845685653e-16, 4.995761974265609e-07, -8.660276935385502e-21, 0.0, 0.0, 0.0, -4.7911713210145933e-14, -8.66027693530199e-21, 4.995761974247493e-07, 0.0, 0.025429218630340738, 0.012462089384510886, 0.0, 0.0, 0.0, 0.04745391147404804]
twist: 
  twist: 
    linear: 
      x: 2.27277520454
      y: -0.00930319565142
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0662314156634
  covariance: [0.030605353242353292, 1.5360603608609234e-05, 0.0, 0.0, 0.0, 1.612949947829346e-05, 1.536060360860891e-05, 0.03162492776314439, 0.0, 0.0, 0.0, -0.0001475585746612767, 0.0, 0.0, 4.996820139817596e-07, 2.5857478835060878e-23, -1.4395350433844195e-23, 0.0, 0.0, 0.0, 2.585747883506089e-23, 4.987328831749013e-07, 1.8865117849549457e-27, 0.0, 0.0, 0.0, -1.43953504338442e-23, 1.8865117730560123e-27, 4.987328831749013e-07, 0.0, 1.6129499478294185e-05, -0.0001475585746612767, 0.0, 0.0, 0.0, 0.03567108187887835]

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

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 link of flowchart of how I fed the signals: https://drive.google.com/file/d/1ZgXKLhqVFdprLXiR9m0XGbrB0QESbUtO/view?usp=sharing image description

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/16QCFkYMWkmBXjNM-OZjVQYwEbE3y0fDD/view?usp=sharing

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" value="20"/>

    <param name="debug" value="false"/>
    <param name="debug_out_file" value="debug_ekf_localization.txt"/>

    <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.01, 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.1,  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,    0,    0,    0,    0,    0.04, 0,    0,    0,    0,    0,    0,    
              0,    0,    0,    0,    0,    0,    0,    0,    0,    0.01, 0,    0,    0,    0,    0,    
              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0.01, 0,    0,    0,    0,    
              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0.01, 0,    0,    0,    
              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0.01, 0,    0,    
              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0.01, 0,    
              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0.01  ]</rosparam>

    <rosparam param="initial_estimate_covariance">
            [1e-6, 0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    
             0,    1e-6, 0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    
             0,    0,    1e-6, 0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    
             0,    0,    0,    1e-6, 0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    
             0,    0,    0,    0,    1e-6, 0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    
             0,    0,    0,    0,    0,    1e-6, 0,    0,    0,    0,    0,    0,    0,    0,    0,    
             0,    0,    0,    0,    0,    0,    1e-6, 0,    0,    0,    0,    0,    0,    0,    0,    
             0,    0,    0,    0,    0,    0,    0,    1e-6, 0,    0,    0,    0,    0,    0,    0,    
             0,    0,    0,    0,    0,    0,    0,    0,    1e-6, 0,    0,    0,    0,    0,    0,    
             0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-6, 0,    0,    0,    0,    0,    
             0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-6, 0,    0,    0,    0,    
             0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-6, 0,    0,    0,    
             0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-6, 0,    0,    
             0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-6, 0,    
             0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-6  ]</rosparam>

    <remap from="/odometry/filtered" to="/odometry/filtered_odom" />
</node>


<node pkg="robot_localization" type="navsat_transform_node" name="vehicle_navsat_transform_node" clear_params="true">

    <!-- Placeholders for input remapping. Set your topic names as the "to" values.
    <remap from="imu/data" to=""/>
    <remap from="odometry/filtered" to=""/>
    <remap from="gps/fix" to=""/>
    -->
    <param name="frequency" value="30" />
    <param name="delay" value="0" />
    <param name="magnetic_declination_radians" value="-0.249625367" />
    <param name="yaw_offset" value="0.22" />
    <param name="zero_altitude" value="true" />
    <param name="broadcast_utm_transform" value="false" />
    <param name="publish_filtered_gps" value="false" />
    <param name="use_odometry_yaw" value="true" />
    <param name="wait_for_datum" value="true" />
    <rosparam param="datum">[42.275921, -71.798370, 0.0]</rosparam>
    <!--
      input: 
           1. fused odom data from ekf_odom_1: 
            /odometry/filtered -> /odometry/filtered_odom
           2. raw gps data from vehicle's GPS driver:
              /gps/fix -> /vehicle/gps/fix2
      -->
    <remap from="/odometry/filtered" to="/odometry/filtered_odom" />
    <remap from="/gps/fix" to="/vehicle/gps/fix2" />
    <!--
      output:
           1. fused GPS message:
              /gps/filtered -> /gps/filtered_odom_vehicle
           2. fused odom message: (#FIXME: looks wrong, therefore not be used!)
          /odometry/gps -> /odometry/filtered_odom_vehicle
      -->
    <remap from="/odometry/gps" to="/odometry/filtered_odom_vehicle" />
    <remap from="/gps/filtered" to="/gps/filtered_odom_vehicle" />

</node>
</launch>

Sample Data:

From IMU1: (rostopic: /vehicle/imu/data_raw)

header: 
  seq: 312698
  stamp: 
    secs: 1521833157
    nsecs: 194273627
  frame_id: "base_footprint"
orientation: 
  x: 0.0
  y: 0.0
  z: 0.0
  w: 0.0
orientation_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: 0.0004
  y: 0.0
  z: 0.005
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration: 
  x: -0.18
  y: 0.02
  z: -9.82
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

From IMU2: (rostopic: /mti/sensor/imu)

header: 
  seq: 1126
  stamp: 
    secs: 3583
    nsecs: 537100000
  frame_id: "mti/data"
orientation: 
  x: 0.0
  y: 0.0
  z: 0.0
  w: 0.0
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: -0.00785939209163
  y: -0.0112495776266
  z: 0.015035610646
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration: 
  x: -0.409770280123
  y: -0.113449186087
  z: 9.92660903931
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

raw Odom from wheel encoder: (rostopic: /vehicle/odom/data_raw)

header: 
  seq: 3510
  stamp: 
    secs: 1523511193
    nsecs: 254081010
  frame_id: "odom"
child_frame_id: "base_link"
pose: 
  pose: 
    position: 
      x: 25.0594859288
      y: -32.5312823615
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.390051939181
      w: 0.920792856587
  covariance: [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, 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]
twist: 
  twist: 
    linear: 
      x: 2.51111102104
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.346135645213
  covariance: [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, 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]

From GPS_driver: (rostopic: /vehicle/gps/fix2)

header: 
  seq: 21
  stamp: 
    secs: 1523511141
    nsecs: 589173078
  frame_id: "base_link"
status: 
  status: 0
  service: 0
latitude: 42.2756033333
longitude: -71.7981716667
altitude: 152.25
position_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
position_covariance_type: 0

Output from navsat_transform node: (rostopic: /odometry/filtered_odom_vehicle)

header: 
  seq: 2442
  stamp: 
    secs: 1523511203
    nsecs:   5774975
  frame_id: "map"
child_frame_id: "base_link"
pose: 
  pose: 
    position: 
      x: 21.0497827582
      y: -2.52271801714
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.792082581899
      w: 0.610413944347
  covariance: [0.04228150575870658, 0.006547072346641349, 0.0, 0.0, 0.0, -0.029262488820821206, 0.006547072346641346, 0.018975901890026334, 0.0, 0.0, 0.0, -0.007661683197784052, 0.0, 0.0, 4.987548663770124e-07, 1.4502935722807452e-19, -6.941216635616791e-14, 0.0, 0.0, 0.0, 1.4502935722807271e-19, 4.995835707030885e-07, 8.085457769023848e-24, 0.0, 0.0, 0.0, -6.941216635616789e-14, 8.085457746617754e-24, 4.99583570699219e-07, 0.0, -0.0292624888208212, -0.0076616831977840485, 0.0, 0.0, 0.0, 0.044752510509221695]
twist: 
  twist: 
    linear: 
      x: 3.34638040916
      y: -0.000663673175027
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0427353974701
  covariance: [0.029937610018694723, -1.748788884986028e-06, 0.0, 0.0, 0.0, -1.9526198758763966e-06, -1.7487888849861702e-06, 0.031781089816912814, 0.0, 0.0, 0.0, -0.00029328346609445733, 0.0, 0.0, 4.996875475492092e-07, -4.146782564862857e-23, 1.0922949666341516e-23, 0.0, 0.0, 0.0, -4.146782564862858e-23, 4.987548512708783e-07, 2.3359823844380062e-27, 0.0, 0.0, 0.0, 1.0922949666341511e-23, 2.335982408268371e-27, 4.987548512708783e-07, 0.0, -1.9526198758802387e-06, -0.0002932834660944581, 0.0, 0.0, 0.0, 0.035568126302575054]

Output filtered estimation: (rostopic: /odometry/filtered_odom)

header: 
  seq: 405
  stamp: 
    secs: 1523512494
    nsecs: 659452915
  frame_id: "map"
child_frame_id: "base_link"
pose: 
  pose: 
    position: 
      x: 4.3553063759
      y: -30.6477541417
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: -0.506579466828
      w: 0.862193275193
  covariance: [0.04513444652848379, 0.0094500678928774, 0.0, 0.0, 0.0, 0.025429218630340738, 0.009450067892877393, 0.03012814700393296, 0.0, 0.0, 0.0, 0.012462089384510879, 0.0, 0.0, 4.987328908167554e-07, -2.290360084568565e-16, -4.7911713210145933e-14, 0.0, 0.0, 0.0, -2.2903600845685653e-16, 4.995761974265609e-07, -8.660276935385502e-21, 0.0, 0.0, 0.0, -4.7911713210145933e-14, -8.66027693530199e-21, 4.995761974247493e-07, 0.0, 0.025429218630340738, 0.012462089384510886, 0.0, 0.0, 0.0, 0.04745391147404804]
twist: 
  twist: 
    linear: 
      x: 2.27277520454
      y: -0.00930319565142
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0662314156634
  covariance: [0.030605353242353292, 1.5360603608609234e-05, 0.0, 0.0, 0.0, 1.612949947829346e-05, 1.536060360860891e-05, 0.03162492776314439, 0.0, 0.0, 0.0, -0.0001475585746612767, 0.0, 0.0, 4.996820139817596e-07, 2.5857478835060878e-23, -1.4395350433844195e-23, 0.0, 0.0, 0.0, 2.585747883506089e-23, 4.987328831749013e-07, 1.8865117849549457e-27, 0.0, 0.0, 0.0, -1.43953504338442e-23, 1.8865117730560123e-27, 4.987328831749013e-07, 0.0, 1.6129499478294185e-05, -0.0001475585746612767, 0.0, 0.0, 0.0, 0.03567108187887835]

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

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: image description

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 ROS bag File: *File: https://drive.google.com/file/d/16QCFkYMWkmBXjNM-OZjVQYwEbE3y0fDD/view?usp=sharing

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" value="20"/>

    <param name="debug" value="false"/>
    <param name="debug_out_file" value="debug_ekf_localization.txt"/>

    <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.01, 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.1,  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,    0,    0,    0,    0,    0.04, 0,    0,    0,    0,    0,    0,    
              0,    0,    0,    0,    0,    0,    0,    0,    0,    0.01, 0,    0,    0,    0,    0,    
              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0.01, 0,    0,    0,    0,    
              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0.01, 0,    0,    0,    
              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0.01, 0,    0,    
              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0.01, 0,    
              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0.01  ]</rosparam>

    <rosparam param="initial_estimate_covariance">
            [1e-6, 0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    
             0,    1e-6, 0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    
             0,    0,    1e-6, 0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    
             0,    0,    0,    1e-6, 0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    
             0,    0,    0,    0,    1e-6, 0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    
             0,    0,    0,    0,    0,    1e-6, 0,    0,    0,    0,    0,    0,    0,    0,    0,    
             0,    0,    0,    0,    0,    0,    1e-6, 0,    0,    0,    0,    0,    0,    0,    0,    
             0,    0,    0,    0,    0,    0,    0,    1e-6, 0,    0,    0,    0,    0,    0,    0,    
             0,    0,    0,    0,    0,    0,    0,    0,    1e-6, 0,    0,    0,    0,    0,    0,    
             0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-6, 0,    0,    0,    0,    0,    
             0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-6, 0,    0,    0,    0,    
             0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-6, 0,    0,    0,    
             0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-6, 0,    0,    
             0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-6, 0,    
             0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-6  ]</rosparam>

    <remap from="/odometry/filtered" to="/odometry/filtered_odom" />
</node>


<node pkg="robot_localization" type="navsat_transform_node" name="vehicle_navsat_transform_node" clear_params="true">

    <!-- Placeholders for input remapping. Set your topic names as the "to" values.
    <remap from="imu/data" to=""/>
    <remap from="odometry/filtered" to=""/>
    <remap from="gps/fix" to=""/>
    -->
    <param name="frequency" value="30" />
    <param name="delay" value="0" />
    <param name="magnetic_declination_radians" value="-0.249625367" />
    <param name="yaw_offset" value="0.22" />
    <param name="zero_altitude" value="true" />
    <param name="broadcast_utm_transform" value="false" />
    <param name="publish_filtered_gps" value="false" />
    <param name="use_odometry_yaw" value="true" />
    <param name="wait_for_datum" value="true" />
    <rosparam param="datum">[42.275921, -71.798370, 0.0]</rosparam>
    <!--
      input: 
           1. fused odom data from ekf_odom_1: 
            /odometry/filtered -> /odometry/filtered_odom
           2. raw gps data from vehicle's GPS driver:
              /gps/fix -> /vehicle/gps/fix2
      -->
    <remap from="/odometry/filtered" to="/odometry/filtered_odom" />
    <remap from="/gps/fix" to="/vehicle/gps/fix2" />
    <!--
      output:
           1. fused GPS message:
              /gps/filtered -> /gps/filtered_odom_vehicle
           2. fused odom message: (#FIXME: looks wrong, therefore not be used!)
          /odometry/gps -> /odometry/filtered_odom_vehicle
      -->
    <remap from="/odometry/gps" to="/odometry/filtered_odom_vehicle" />
    <remap from="/gps/filtered" to="/gps/filtered_odom_vehicle" />

</node>
</launch>

Sample Data:

From IMU1: (rostopic: /vehicle/imu/data_raw)

header: 
  seq: 312698
  stamp: 
    secs: 1521833157
    nsecs: 194273627
  frame_id: "base_footprint"
orientation: 
  x: 0.0
  y: 0.0
  z: 0.0
  w: 0.0
orientation_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: 0.0004
  y: 0.0
  z: 0.005
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration: 
  x: -0.18
  y: 0.02
  z: -9.82
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

From IMU2: (rostopic: /mti/sensor/imu)

header: 
  seq: 1126
  stamp: 
    secs: 3583
    nsecs: 537100000
  frame_id: "mti/data"
orientation: 
  x: 0.0
  y: 0.0
  z: 0.0
  w: 0.0
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: -0.00785939209163
  y: -0.0112495776266
  z: 0.015035610646
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration: 
  x: -0.409770280123
  y: -0.113449186087
  z: 9.92660903931
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

raw Odom from wheel encoder: (rostopic: /vehicle/odom/data_raw)

header: 
  seq: 3510
  stamp: 
    secs: 1523511193
    nsecs: 254081010
  frame_id: "odom"
child_frame_id: "base_link"
pose: 
  pose: 
    position: 
      x: 25.0594859288
      y: -32.5312823615
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.390051939181
      w: 0.920792856587
  covariance: [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, 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]
twist: 
  twist: 
    linear: 
      x: 2.51111102104
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.346135645213
  covariance: [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, 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]

From GPS_driver: (rostopic: /vehicle/gps/fix2)

header: 
  seq: 21
  stamp: 
    secs: 1523511141
    nsecs: 589173078
  frame_id: "base_link"
status: 
  status: 0
  service: 0
latitude: 42.2756033333
longitude: -71.7981716667
altitude: 152.25
position_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
position_covariance_type: 0

Output from navsat_transform node: (rostopic: /odometry/filtered_odom_vehicle)

header: 
  seq: 2442
  stamp: 
    secs: 1523511203
    nsecs:   5774975
  frame_id: "map"
child_frame_id: "base_link"
pose: 
  pose: 
    position: 
      x: 21.0497827582
      y: -2.52271801714
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.792082581899
      w: 0.610413944347
  covariance: [0.04228150575870658, 0.006547072346641349, 0.0, 0.0, 0.0, -0.029262488820821206, 0.006547072346641346, 0.018975901890026334, 0.0, 0.0, 0.0, -0.007661683197784052, 0.0, 0.0, 4.987548663770124e-07, 1.4502935722807452e-19, -6.941216635616791e-14, 0.0, 0.0, 0.0, 1.4502935722807271e-19, 4.995835707030885e-07, 8.085457769023848e-24, 0.0, 0.0, 0.0, -6.941216635616789e-14, 8.085457746617754e-24, 4.99583570699219e-07, 0.0, -0.0292624888208212, -0.0076616831977840485, 0.0, 0.0, 0.0, 0.044752510509221695]
twist: 
  twist: 
    linear: 
      x: 3.34638040916
      y: -0.000663673175027
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0427353974701
  covariance: [0.029937610018694723, -1.748788884986028e-06, 0.0, 0.0, 0.0, -1.9526198758763966e-06, -1.7487888849861702e-06, 0.031781089816912814, 0.0, 0.0, 0.0, -0.00029328346609445733, 0.0, 0.0, 4.996875475492092e-07, -4.146782564862857e-23, 1.0922949666341516e-23, 0.0, 0.0, 0.0, -4.146782564862858e-23, 4.987548512708783e-07, 2.3359823844380062e-27, 0.0, 0.0, 0.0, 1.0922949666341511e-23, 2.335982408268371e-27, 4.987548512708783e-07, 0.0, -1.9526198758802387e-06, -0.0002932834660944581, 0.0, 0.0, 0.0, 0.035568126302575054]

Output filtered estimation: (rostopic: /odometry/filtered_odom)

header: 
  seq: 405
  stamp: 
    secs: 1523512494
    nsecs: 659452915
  frame_id: "map"
child_frame_id: "base_link"
pose: 
  pose: 
    position: 
      x: 4.3553063759
      y: -30.6477541417
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: -0.506579466828
      w: 0.862193275193
  covariance: [0.04513444652848379, 0.0094500678928774, 0.0, 0.0, 0.0, 0.025429218630340738, 0.009450067892877393, 0.03012814700393296, 0.0, 0.0, 0.0, 0.012462089384510879, 0.0, 0.0, 4.987328908167554e-07, -2.290360084568565e-16, -4.7911713210145933e-14, 0.0, 0.0, 0.0, -2.2903600845685653e-16, 4.995761974265609e-07, -8.660276935385502e-21, 0.0, 0.0, 0.0, -4.7911713210145933e-14, -8.66027693530199e-21, 4.995761974247493e-07, 0.0, 0.025429218630340738, 0.012462089384510886, 0.0, 0.0, 0.0, 0.04745391147404804]
twist: 
  twist: 
    linear: 
      x: 2.27277520454
      y: -0.00930319565142
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0662314156634
  covariance: [0.030605353242353292, 1.5360603608609234e-05, 0.0, 0.0, 0.0, 1.612949947829346e-05, 1.536060360860891e-05, 0.03162492776314439, 0.0, 0.0, 0.0, -0.0001475585746612767, 0.0, 0.0, 4.996820139817596e-07, 2.5857478835060878e-23, -1.4395350433844195e-23, 0.0, 0.0, 0.0, 2.585747883506089e-23, 4.987328831749013e-07, 1.8865117849549457e-27, 0.0, 0.0, 0.0, -1.43953504338442e-23, 1.8865117730560123e-27, 4.987328831749013e-07, 0.0, 1.6129499478294185e-05, -0.0001475585746612767, 0.0, 0.0, 0.0, 0.03567108187887835]