odometry/filtered from robot_localization not matching orientation input from imu
I'm doing 3D state estimation with r_l and having some trouble. I have 3 sensors that I'm fusing:
- 2D wheel encoder based wheel odometry
- zed camera odom, spews both xyz and rpy. seems like rpy is a little unreliable so I'm only using xyz
- rpy from a relative imu
Looking at my fused data, my rpy appears to be all of the place and not using anything from the imu. In fact, I get the same results if I simply disable my imu0 in the r_l yaml file. What's going on with my data? Is the covariance of the imu too big such that the EKF is simply ignoring it?
here is my yaml file:
initial_state: [0.0, 0.0, 0.59,
0.0, 0.0, 0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0]
frequency: 10
two_d_mode: false
transform_time_offset: 0.2
transform_timeout: 0.0
print_diagnostics: false
debug: false
debug_out_file: /tmp/debug.txt
publish_tf: true
publish_acceleration: false
predict_to_current_time: 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: wheel_odom
#only use velocities from wheel_odom, abs val are erroneous due to slip. only vx,vy and vyaw
odom0_config: [false, false, false,
false, false, false,
true, true, false,
false, false, false,
false, false, false]
odom0_queue_size: 5
odom0_differential: false
odom0_relative: false
use_control: false
#bottom facing VO camera
#using everything from vo for now
odom1: /camera_manager1/zed_node/odom
odom1_config: [true, true, true,
false,false, false,
false,false, false,
false,false, false,
false,false, false]
odom1_differential: false
odom1_relative: false
#
imu0: /vectornav/IMU
#The IMU gives orientation, linear acceleration, and angular velocity
#use imu for velocity for vx,vy,vz, vrpy
imu0_config: [false, false, false,
true, true, true,
false, false, false,
false, false, false,
false, false, false]
imu0_differential: false
imu0_relative: true
imu0_remove_gravitational_acceleration: true
bag is here: https://drive.google.com/file/d/10l0J...
wheel_odom:
header:
seq: 286175
stamp:
secs: 1605734718
nsecs: 849224511
frame_id: "odom"
child_frame_id: "base_link"
pose:
pose:
position:
x: 1.26266401924
y: -4.65020220554
z: 0.572850814038
orientation:
x: 0.0
y: 0.0
z: -0.914122786186
w: 0.405437457292
covariance: [0.025, 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, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.14, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.14, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.11]
twist:
twist:
linear:
x: -0.0172806155128
y: -0.0188698232587
z: 0.0
angular:
x: 0.0
y: 0.0
z: -0.0459902520252
covariance: [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, 1.0, 0.0 ...
Hi there, please include a sample message from every sensor input.