/odometry/gps and /odometry/filtered output is drifting

asked 2020-01-06 03:58:22 -0600

vijay123
11 ●2 ●3 ●4

updated 2020-02-12 04:16:31 -0600

Hi, I am using robot localization package along with only GPS and IMU sensor to find out the position,

Below is my launch file

    <launch>
       <node pkg="tf2_ros" type="static_transform_publisher" name="bl_imu" args = "0 0 0 0 0 0 1 base_link imu">
    </node>

   <node pkg="tf2_ros" type="static_transform_publisher" name="bl_gps" args = "0 0 0 0 0 0 1 base_link gps_link">
    </node>

  <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true" output="screen">
    <rosparam command="load" file="$(find robot_localization)/params/navsat_transform_template.yaml" />
      <!-- Placeholders for input remapping. Set your topic names as the "to" values.-->
     <remap from="/imu/data" to="/imu/data" />
    <remap from="/gps/fix" to="/fix" />
    <remap from="/odometry/filtered" to="/odometry/filtered"/>
  </node>

   <node pkg="robot_localization" type="ekf_localization_node" name="ekf_odom" clear_params="true" output="screen">
      <rosparam command="load" file="$(find robot_localization)/params/ekf_template.yaml" />

  </node>
</launch>

my YAML configuration file is as below,

   frequency: 30
    sensor_timeout: 0.1

    two_d_mode: true
    transform_time_offset: 0.0
    transform_timeout: 0.0
    print_diagnostics: true
    debug: false

    debug_out_file: /path/to/debug/file.txt.
    publish_tf: true
    publish_acceleration: false

    map_frame: map              
    odom_frame: odom          
    base_link_frame: base_link  
    world_frame: odom         

   imu0: imu/data
     imu0_config: [false, false, false,
              false, false, true,
              false, false, false,
              false, false, true,
              false, false, false]

    imu0_nodelay: false
    imu0_differential: false
    imu0_relative: true
    imu0_queue_size: 10
    imu0_pose_rejection_threshold: 0.8                 # Note the difference in parameter names
    imu0_twist_rejection_threshold: 0.8                #
    imu0_linear_acceleration_rejection_threshold: 0.8  
    imu0_remove_gravitational_acceleration: true


    odom0: /odom
   odom0_config: [false, false, false,
               false, false, false,
               true, true, false,
               false, false, true,
               false, false, false]
    odom0_queue_size: 10
    odom0_nodelay: false
    odom0_differential: false
    odom0_relative: true
    odom0_pose_rejection_threshold: 5
    odom0_twist_rejection_threshold: 1

odom1: /odometry/gps
odom1_config: [true, true, false,
               false, false, false,
               false, false, false,
               false, false, false,
               false, false, false]

odom1_differential: false
odom1_relative: true
odom1_queue_size: 10
odom1_pose_rejection_threshold: 2
odom1_twist_rejection_threshold: 0.2
odom1_nodelay: false

    imu0_remove_gravitational_acceleration: true

    use_control: false
    stamped_control: false.
    control_timeout: 0.2

    control_config: [true, false, false, false, false, true]
    acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
    deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
    acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
    deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]

    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.06, 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.06, 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 ...

(more)

Comments

@vijay123 your question is very hard to read due to you not using the syntax highlighting. Can you please edit your question to enable it? (Highlight the code/text then press the preformatted text (101010) button.)

jayess ( 2020-02-06 14:07:43 -0600 )edit

@jayess Thanks for observation. I have formatted the question and attached the bag file as well. you can have look on the data.

vijay123 ( 2020-02-12 03:58:12 -0600 )edit

add a comment

answered 2020-01-20 03:23:51 -0600

Tom Moore

flag of United Kingdom of Great Britain and Northern Ireland

13689 ●74 ●240 ●232 http://github.com/ayrton04

So your GPS data is _not_ given in the odom frame. If they were the same frame, you could fuse them directly, without the need for navsat_transform_node.

GPS devices are a bit strange, because they give world-fixed pose information, but we have to define a vehicle-relative transform/offset for them. navsat_transform_node makes the assumption that the GPS is mounted somewhere on your robot. If it's mounted directly in the center of the vehicle, then you can make its frame base_link. If it's mounted, e.g., 0.1 meters in front of the robot, then you need to define that in a static transform, and change the GPS message frame_id to gps (or whatever you want to name it). Keep in mind that the static transform for the GPS should _not_ have a rotation, as the orientation of the GPS sensor is meaningless.

One other note: you are just fusing IMU and GPS data. The filter won't play well with that, because it lacks bias estimation. You need a velocity reference. You have use_control turned on, but we are modelling controls as accelerations in the filters, so that won't help. If you have a wheel odometry source, feed that to the filter. If you don't, then take your motor command topic, and publish a TwistWithCovarianceStamped message with that data, and feed it to the filter as a velocity input. It'll help keep things on track.

edit flag offensive delete link

Comments

thanks. I have changed gps data frame-id to gps_link and performed static transform by launching another node and when i check the output of odometry/gps topic it is continuously drifting.As of now i dont have robot and i am only trying to get familiar with r_l using imu and gps. In order to provide the velocity as you recommend, i am publishing TwistWithCovarianceStamped msg with data as zero.

Independently i have verified navsat node's utm output is satisfactory.

I would like to know how we can validate the output of navsat and ekf nodes output

vijay123 ( 2020-02-03 06:19:51 -0600 )edit

If the filtered GPS data is continually drifting, then your EKF state estimate is continually drifting, which makes me question whether it is properly subscribed to your twist message.

Also, is your GPS sensor itself also continually drifting? Your EKF is fusing the GPS data into its state estimate. It then sends that fused data to navsat_transform_node, which converts it back to GPS data and publishes. But if your GPS is drifting, your state estimate is also going to drift. And if it's not respecting your velocity reference (TwistWithCovarianceStamped message), it'll drift an insane amount, because you only have a GPS and IMU.

Tom Moore ( 2020-02-11 03:28:31 -0600 )edit

yes my understanding is same. I have attached bag file with all topics and other details. you can have a look. one experiment i did where i have given fix IMU data and actual GPS data odometry/gps is drifting where as if we fix the GPS data and give actual IMU data odometry/gps is not drifting. any parameter tuning is required?

vijay123 ( 2020-02-12 04:08:05 -0600 )edit

@Tom Moore ,when and in what case it will not respect velocity reference (TwistWithCovarianceStamped message) and how to solve this problem???

vijay123 ( 2020-02-18 03:43:41 -0600 )edit

Post a sample message, please.

Tom Moore ( 2020-02-25 01:57:45 -0600 )edit

add a comment

/odometry/gps and /odometry/filtered output is drifting

Comments

1 Answer

Comments

Question Tools

Stats

Related questions

/odometry/gps and /odometry/filtered output is drifting edit

Comments

1 Answer

Comments

Question Tools

Stats

Related questions

/odometry/gps and /odometry/filtered output is drifting