Robotics StackExchange | Archived questions

Odometry/Filtered of robot_localization is not computing anything

I am using robotlocalization for fusing two input sources. one is a sensor which gives me only x and y positioning information and other is odometry. I have translated both in navmsgs/oodmetry format with two different topic names. When i use robotlocalization with them, it only publishes the exact values, not computing anything. Also if i use positioning information from both of my inputs, it only publishes the exact value of one, completely ignoring the other. Below i have both my msg outputs, Odometry/filtered output and my ekftemplate.yaml file. Where i am doing wrong and why is extended kalman filter not computing anything? Ekf file:

frequency: 30
sensor_timeout: 10
two_d_mode: false
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
debug_out_file: /home/shairi/catkin_ws/src/debug.txt
publish_tf: true
publish_acceleration: false



#map_frame: map             
odom_frame: odom            
base_link_frame: base_footprint  
world_frame: odom           
base_link_output_frame: base_link
odom0: /odom

odom0_config: [true, true,  false,
               false, false, true,
               false, false, false,
               false, false, false,
               false, false, false]

odom0_queue_size: 5

odom0_nodelay: false

odom0_differential: false

odom0_relative: false
odom1: /odom1
odom1_config: [true, true, false,
               false, false, false,
               false, false, false,
               false, false, false,
               false, false, false]
odom1_differential: false
odom1_relative: false
odom1_queue_size: 2

odom1_nodelay: false


use_control: false
stamped_control: false
control_timeout: 0.2
control_config: [true, false, false, false, false, false]
acceleration_limits: [0, 0.0, 0.0, 0.0, 0.0, 0]
deceleration_limits: [0, 0.0, 0.0, 0.0, 0.0, 0]
acceleration_gains: [0.5, 0.0, 0.0, 0.0, 0.0, 0.5]
deceleration_gains: [0.5, 0.0, 0.0, 0.0, 0.0, 0.5]

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,    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.02, 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.015]


initial_estimate_covariance: [0.2, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0.2, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0.2, 0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-9,  0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-9,  0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1e-9,  0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1e-9, 0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-9, 0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]

Sensor output:::

header: 
  seq: 2219
  stamp: 
    secs: 1571320316
    nsecs: 593163013
  frame_id: "odom"
child_frame_id: "base_footprint"
pose: 
  pose: 
    position: 
      x: 0.667387096774
      y: -0.0459105691057
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.0
      w: 0.0
  covariance: [0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
twist: 
  twist: 
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

Odometry output::

header: 
  seq: 547
  stamp: 
    secs: 1571320310
    nsecs: 763436079
  frame_id: "odom"
child_frame_id: "base_footprint"
pose: 
  pose: 
    position: 
      x: 0.696610093117
      y: 0.0416755899787
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.0037443251349
      w: 0.999992966652
  covariance: [0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
twist: 
  twist: 
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.000284984707832
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
---

Odometry/filtered:::

header: 
  seq: 291
  stamp: 
    secs: 1571320694
    nsecs: 160778999
  frame_id: "odom"
child_frame_id: "base_footprint"
pose: 
  pose: 
    position: 
      x: 0.667387106415
      y: -0.0459105691135
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.000106268933876
      w: 0.999999994353
  covariance: [0.07293236604371, -8.793154904565299e-18, 0.0, 0.0, 0.0, 4.3810840046446096e-11, -8.793578421038926e-18, 0.07293236604374022, 0.0, 0.0, 0.0, -1.4971443808366921e-07, 0.0, 0.0, 68.33309228549892, 1.8300913865127743e-10, -8.556398526587704e-06, 0.0, 0.0, 0.0, 1.8300913865127644e-10, 3.106966214605023, 1.3541939244228127e-19, 0.0, 0.0, 0.0, -8.556398526587703e-06, -1.3541939244228127e-19, 3.106966214605023, 0.0, 4.381084004644611e-11, -1.4971443808366916e-07, 0.0, 0.0, 0.0, 0.774107822390654]
twist: 
  twist: 
    linear: 
      x: -2.60323292052e-07
      y: -1.53570340739e-13
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: -0.000106579748269
  covariance: [0.039415175227834796, 2.072975650062514e-20, 0.0, 0.0, 0.0, -1.7882735077309876e-31, 2.0950081276998254e-20, 0.03941517522783673, 0.0, 0.0, 0.0, 2.945587834530402e-26, 0.0, 0.0, 4.612610734111524, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.09567607025964361, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.09567607025964361, 0.0, -1.7882735077310146e-31, 2.9455878345304035e-26, 0.0, 0.0, 0.0, 9.999998017635656e-10]

Asked by enthusiast.australia on 2019-10-17 09:20:18 UTC

Comments

Answers

A couple things:

  • Are the time stamps on your inputs correct? Is one very old?
  • Even if the filter was respecting both, you are fusing two sources of absolute pose data. One is coming from odometry, so over time, they won't agree with each other, so the filter will start jumping back and forth between them.

You need to fuse X and Y position from your XY positioning data sensor, and X, Y, and yaw velocity from your odometry. Even then, you'll eventually run into trouble, because your yaw will drift, and your true heading (as determined by using atan on your XY positioning data) won't match the robot's odometry heading. Can you compute a heading for the odom1 data source?

Asked by Tom Moore on 2020-01-02 05:14:59 UTC

Comments