Using only velocity in robot_localization
Hey guys,
I'm using robot_localization ekf on ROS2 Foxy to fuse two odometry sources. I am working in 2D so only x, y and yaw is used and the two_d_mode is set to true. I was trying to use only the velocities because the best practice (told on the docu) is to fuse the velocity data if your odometry gives you both. If I do so the Filter doesnt output an odometry source (even with sensor_relative set to true).
I tried using x_vel, y_vel, and yaw_vel on both and also combinations of only using x_vel on the first and the other two on the second odom source for example.
When using the position of one odometry source and the velocity of the other its working fine, as expected. So the setup should be alright.
Do I always have to fuse position data to get it working? Shouldnt the EKF work without position data when the relative parameter is true and its starts at 0?
Thanks for you help in advance!
Update with my settings and sample sensor messages: The only message I get when trying to use 2 velocities and relative parameter true (same case with this parameter off but I dont know how it should work without the relative state so I thought turning it on is the right move) is the following. That message doesnt show up when using pose data from one sensor.
[ekf_node-1] Warning: Invalid frame ID "base_link" passed to canTransform argument target_frame - frame does not exist
[ekf_node-1] at line 133 in /home/dominik/ros2_ws/src/geometry2/tf2/src/buffer_core.cpp
My settings: I cut off everything on the bottom, beginning with the use_control parameter (which is false), so the thread doesnt get longer than needed. Everything there is untouched, so its default.
ekf_filter_node:
ros__parameters:
frequency: 30.0
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: /home/dominik/debug/robot_localization_debug.txt
publish_tf: true
publish_acceleration: true
#map_frame: map # Defaults to "map" if unspecified
odom_frame: odom # Defaults to "odom" if unspecified
base_link_frame: base_link # Defaults to "base_link" if unspecified
world_frame: odom # Defaults to the value of odom_frame if unspecified
odom0: camera/odom
odom0_config: [false, false, false,
false, false, false,
true, true, false,
false, false, true,
false, false, false]
odom0_queue_size: 2
odom0_nodelay: false
odom0_differential: false
odom0_relative: true
odom0_pose_rejection_threshold: 5.0
odom0_twist_rejection_threshold: 1.0
odom1: wheel/odom
odom1_config: [false, false, false,
false, false, false,
true, true, false,
false, false, true,
false, false, false]
odom1_differential: false
odom1_relative: true
odom1_queue_size: 2
odom1_pose_rejection_threshold: 2.0
odom1_twist_rejection_threshold: 0.2
odom1_nodelay: false
Sample from /camera/odom
header:
stamp:
sec: 1611602305
nanosec: 640852033
frame_id: odom
child_frame_id: camera_base_link
pose:
pose:
position:
x: -0.38118633360398285
y: -3.465815506686516e-05
z: -0.05412765641582427
orientation:
x: 0.00031491834226558634
y: 0.006867443114608576
z: 0.0003045650621801162
w: 0.9999763228655107
covariance:
- 1.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 1.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 1.0
- 0.0
- 0.0
- 0 ...
@pfedom the "relative" parameter specifies:
Why do you think that has to do with your problem?
My thought was that the Kalman Filter needs an initial pose. Normally given by the pose of an odometry. But if I only fuse velocities it doesnt know where to start. Anyways it didnt work. Do you know how to get it going without pose information given by a sensor?
Unless you specify the
~initial_state
parameter it just starts everything at 0. So it always knows where to start. I really don't think you need pose information, and I can't say exactly what the problem is with the information provided here, and I don't have time to run it myself rn to checkThe state equations for the position state are essentially Newtons law of motion so,
so it seems like there is a mistake somewhere else in the pipeline as to why this wont work. Because as you can see the position x_{i+1} is clearly linearly related to v_{x}.
you are completely correct with your points. Thats why I thought it should work.
Please post your full config, as well as a sample message from every sensor input.
updated my question. If you need more information just ping me again here
As stated in the docs, unless you are a power user, do not use the rejection thresholds, at least not to start.
I have not changed this. That's the default setting.