ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

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

asked 2018-04-12 00:56:30 -0500

James786 gravatar image

updated 2018-04-13 14:02:42 -0500

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 bag File:

Launch file:

<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 ...
edit retag flag offensive close merge delete


Please attach all images to your question directly. I've give you enough karma to do that.

gvdhoorn gravatar image gvdhoorn  ( 2018-04-12 02:23:20 -0500 )edit

Discrete jumps are expected when you use GPS - especially when your GPS covariance is 0. If you can post a bag file it would help identify other problems.

stevejp gravatar image stevejp  ( 2018-04-12 07:08:15 -0500 )edit

I just edited the answer and posted the bag file. The GPS covariance is zero as it comes from driver. Please check this out. Thanks!

James786 gravatar image James786  ( 2018-04-13 14:05:00 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2018-04-19 05:39:27 -0500

Tom Moore gravatar image

This is expected behavior.

When fusing measurements, an EKF is concerned primarily with two quantities:

  1. The error (covariance) in the state estimate
  2. The error (covariance) in the measurement

So, as you drive your robot around, if you have no pose data coming in, the pose error increases in your state estimate as you integrate velocity measurements. This is what happens between your GPS measurements. Then you receive a GPS measurement, and the filter does a weighted average between that GPS data and your current state estimate. The weights it uses are based on the relative covariances. So if your GPS is jumping around, then unless the covariances are accurate, you're going to see a fair amount of jumping around.

So, to that end, you need to fill out covariance data in all your sensor measurements. Even then, the GPS is very likely to violate the covariance it reports, so your motion won't always be smooth.

This is the very reason for having two separate world frames (e.g., _map_ and _odom_), and for running two EKFs. The _odom_-frame EKF fuses only continuous data, and its output is used by, e.g., the local planner in move_base. The global pose estimate is not subject to drift, thanks to the GPS, but it IS subject to discrete discontinuities.

I recommend reading over REP-105, if you haven't yet.

edit flag offensive delete link more

Question Tools



Asked: 2018-04-12 00:49:18 -0500

Seen: 705 times

Last updated: Apr 19 '18