Ask Your Question
1

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

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

vijay123 gravatar image

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

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 gravatar imagejayess ( 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 gravatar imagevijay123 ( 2020-02-12 03:58:12 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
1

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

Tom Moore gravatar image

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 more

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 gravatar imagevijay123 ( 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 gravatar imageTom 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 gravatar imagevijay123 ( 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 gravatar imagevijay123 ( 2020-02-18 03:43:41 -0600 )edit

Post a sample message, please.

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

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2020-01-06 03:47:23 -0600

Seen: 82 times

Last updated: Feb 12