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
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