robot_localization problem for fuse imu and odometry
Hi all,
I'm trying to fuse IMU and VISP for a drone in Gazebo. The IMU is official plug-in from Gazebo tutorials (GazeboRosImuSensor), and VISP is a stand alone package which can return the pose (x, y, z, quaternion) of camera w.r.t. Apriltag. I have transformed the VISP-data into nav-msgs/Odometry, but I don't have twist data, so I didn't set it.
The update rate of IMU and VISP are set to 200Hz and 10Hz. The problem is the IMU seems like can't "catch up" the movement. The result of EKF jumped every 0.1 secs. For example, the pose increased every 0.005 secs very slowly (from 0.23 to 0.25), and every 0.1 secs the pose has a great increase (form 0.25 to 0.40). I think the possible reason is: every 0.1 secs the EKF update it's current measurement from VISP, which is more likely to be the real state of robot, but the IMU can't predict the robot pose correctly.
Are there anyone have met similar problems? Or anyone has some ideas about that? Thank you for your help.
The msgs and configuration of robot_localization are below. I'm new to this forum and can't upload image and files,sorry about that.
header:
seq: 118
stamp:
secs: 21
nsecs: 679000000
frame_id: "odom"
child_frame_id: "base_link"
pose:
pose:
position:
x: -5.8628621273e-05
y: 3.73823519429e-05
z: 0.221914839373
orientation:
x: 8.49205216555e-06
y: 9.02411111027e-07
z: -1.72706129435e-05
w: 0.999999999814
covariance: [0.01, 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.01, 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.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01]
A twist:
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]
The config file of robot_localization is here:
- frequency: 200
- sensor_timeout: 1.0
- two_d_mode: false
- transform_time_offset: 0.0
- transform_timeout: 0.0
- print_diagnostics: true
- publish_tf: true
- odom_frame: odom
- base_link_frame: base_link
- world_frame: odom
- odom0: /vo
- odom0_config: [true, true, true,
true, true, true,
false, false, false,
false, false, false,
false, false, false]
- odom0_differential: false
- odom0_relative: false
- imu0: /imu
- imu0_config: [false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]
- imu0_differential: false
- imu0_relative: true ...
you cannot attach arbitrary files to posts on ROS Answers (or: Askbot). Only images are supported, and then only if you have sufficient karma (to prevent image spam by new accounts).
Know that images of terminals and source code are not ok.
gvdhoorn, thank you for editing my questions.