Odom frame is shaking and jumping after fusing odometry with IMU
Hello!
I have a question similar to this but that question died. So the situation is I have a four wheeled robot and I am calculating the wheel odom correctly and I want to fuse the yaw from an IMU. This IMU is currently my phone but I don't think it matters. I ran the 2 nodes separately and the tf turns (incorrectly) when the IMU is not present and vica versa.
Config file:
frequency: 30
silent_tf_failure: false
sensor_timeout: 0.01
two_d_mode: true
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
publish_tf: true
publish_acceleration: false
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: /odom
odom0_config: [true, true, false,
false, false, false,
true, false, false,
false, false, false,
false, false, false]
odom0_queue_size: 2
odom0_nodelay: false
odom0_differential: false
odom0_relative: false
#odom0_pose_rejection_threshold: 5
#odom0_twist_rejection_threshold: 1
imu0: /imu0
imu0_config: [false, false, false,
false, false, true,
false, false, false,
false, false, true,
false, false, false]
mu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 5
#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: False
use_control: false
stamped_control: false
control_timeout: 0.2
acceleration_limits: [0.23, 0.0, 0.0, 0.0, 0.0, 0.23]
deceleration_limits: [0.23, 0.0, 0.0, 0.0, 0.0, 0.23]
acceleration_gains: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
deceleration_gains: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
process_noise_covariance:
[0.002, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0.002, 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.002, 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: [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, 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]
Sample messages: /odom0
header:
seq: 2509
stamp:
secs: 1603020401
nsecs: 436315059
frame_id: "odom"
child_frame_id: "base_link"
pose:
pose:
position:
x: 0.226526037924
y: 0.000705410739765
z: 0.0
orientation:
x: 0.0
y: 0.0
z: -0.00151962009824
w: 0.999998845377
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]
twist:
twist:
linear:
x: 0.000158871177273
y: -0.000194978263017
z: 0.0
angular:
x: 0.0
y: 0.0
z: -0.00103887053719
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]
---
/imu0
header:
seq: 712
stamp:
secs: 1603020557
nsecs: 35053253
frame_id: "base_link"
orientation:
x: -0.00594800012186
y: 0.00101300003007
z: -0.995490014553
w: -0.0946670025587
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity:
x: -0.00488692196086
y: -0.00244346098043
z: 0.00122173049022
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration:
x: -0.040300000459
y: 0.107000000775
z: 0.0495999976993
linear_acceleration_covariance: [1000.0, 0.0, 0.0, 0.0, 1000.0, 0.0, 0.0, 0.0, 1000.0]
---
So briefly the error is that it seems like it gets odometry from 2 sources and jumps back and forth and even drifts away sometimes. It's like the filter processes the IMU data too, even when I set it to false. the IMU data is very unstable so I do NOT want to merge it in the final odom, only the orientation. So how can I make correct it so it only takes the linear moving from the wheel odom and the yaw from the IMU?
Any help is appreciated
Asked by istvanbor on 2020-10-18 06:37:47 UTC
Answers
Okay, my bad there was 2 TF sources. It's solved but I can't accept it.
Asked by istvanbor on 2020-10-19 03:49:22 UTC
Comments