robot_localization:fusion of IMU and visual inertial odometry(VIO)

asked 2018-04-02 03:58:18 -0600

Jacky
11 ●4 ●5 ●7

updated 2018-04-08 23:49:39 -0600

Hi, I try to use the acceleration x,y,z from IMU and position x,y,z from VIO to estimate linear velocity x,y,z(on the drone).

At first, when the drone is put on the ground, the result of the estimated linear velocity is normal(linear velocity x, y and z are all approximately zero.) But when I move the drone, the linear velocity starts to jump to large value. And the estimated linear velocity is no longer correct.

However, I try to only use x and z dimensions, and it works well. I also try to only use y dimension, it works well, too. Is there anything wrong with my data or parameters? The following is my sensor data and parameters:

IMU:

header: 
  seq: 1682
  stamp: 
    secs: 1523248805
    nsecs: 647093568
  frame_id: "base_link"
orientation: 
  x: -0.0122626064765
  y: 0.00367087394292
  z: 0.916815569843
  w: -0.399105809831
orientation_covariance: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
angular_velocity: 
  x: 0.000395756680518
  y: -0.000165232922882
  z: -0.00103721336927
angular_velocity_covariance: [1.2184696791468346e-07, 0.0, 0.0, 0.0, 1.2184696791468346e-07, 0.0, 0.0, 0.0, 1.2184696791468346e-07]
linear_acceleration: 
  x: -0.0284779034555
  y: 0.0534957796335
  z: 9.84046649933
linear_acceleration_covariance: [8.999999999999999e-08, 0.0, 0.0, 0.0, 8.999999999999999e-08, 0.0, 0.0, 0.0, 8.999999999999999e-08]

VIO:

header: 
  seq: 2277
  stamp: 
    secs: 1523248919
    nsecs:  87182999
  frame_id: "odom"
pose: 
  pose: 
    position: 
      x: -0.0103095776711
      y: -0.0177321412212
      z: -0.00803798318176
    orientation: 
      x: 0.054222564786
      y: -0.0293635843474
      z: -0.0371067739578
      w: 0.997407028604
  covariance: [2.807622030235894e-05, -2.058458623309457e-05, -3.3721156419156076e-06, 0.997407028604045, -0.010309577671119459, -0.017732141221179724, -0.00803798318175729, -0.0, -0.05422256478599025, 6.95230206988004e-310, 5e-324, 1.91256606e-316, 6.95236747896094e-310, 6.95235731875666e-310, 6.95235117641476e-310, 6.95241666970836e-310, 6.9523573187701e-310, -3.02085629259538e+94, 6.95235117641476e-310, 6.95230206988004e-310, 6.95230206988004e-310, 6.95230206988004e-310, 5e-324, 1.91256606e-316, 6.95236747896094e-310, 6.95236735079744e-310, 2e-323, 6.9523573187598e-310, 6.95235731876377e-310, 6.9523573187527e-310, 7.65732518758802e-207, 6.9523573187602e-310, 6.95235731875666e-310, 6.9523573187551e-310, 6.95230206987925e-310, 6.95230206988004e-310]

Params:Bellow is part of the parameters. If you want to check all the parameters, please see the link. parameters

odom_frame: odom           
base_link_frame: base_link  
world_frame: odom           

pose0: /svo/pose_imu2
pose0_config: [true,  true, true,
               false, false, false,
               false, false, false,
               false, false, false,
               false, false, false]
pose0_differential: false
pose0_relative: false
pose0_queue_size: 1
pose0_rejection_threshold: 2  # Note the difference in parameter name
pose0_nodelay: false

imu0: /drone2/mavros/imu/data
imu0_config: [false, false, false,
              false, false, false,
              false, false, false,
              false, false, false,
              true, true, true]
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 1
imu0_pose_rejection_threshold: 5                 # Note the difference in parameter names
imu0_twist_rejection_threshold: 5               #
imu0_linear_acceleration_rejection_threshold: 8  #

edit retag flag offensive close merge delete

Comments

First, it would be better if you would copy the text instead of putting up photos, as it would help others to refer to this thread.

Akash Purandare ( 2018-04-03 04:50:52 -0600 )edit

add a comment

0

answered 2018-04-03 04:53:22 -0600

Akash Purandare
61 ●1 ●2 ●5

The error increases because of the covariance matrix. You need to calibrate the covariance matrix of linear acceleration to suit the error generated by IMU. What happens when you give small values in Covariance matrix is that robot_localization thinks that your IMU is very accurate and thus the error piles up to become non-decreasing.

edit flag offensive delete link

Comments

But why is that if I only use pose and acceleration of y-axis to estimate y velocity, it looks ok.

Jacky ( 2018-04-08 23:19:21 -0600 )edit

add a comment

0

answered 2018-04-09 03:03:03 -0600

Salahuddin_Khan
50 ●8 ●12 ●18

updated 2018-04-09 04:09:46 -0600

1) In the YAML file at the end of IMU params add

 imu0_remove_gravitational_acceleration: true

So it should look like:

imu0_config: [false, false, false,
          false, false, false,
          false, false, false,
          false, false, false,
          true, true, true]
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 1
imu0_pose_rejection_threshold: 5                 # Note the difference in parameter names
imu0_twist_rejection_threshold: 5               #
imu0_linear_acceleration_rejection_threshold: 8  #
imu0_remove_gravitational_acceleration: true

2) Make sure your static TF of IMU and Camera to the base_link are accurate and confirm if time stamps on accelerations are in sync with VIO

3) Suggestion : write a separate code to differentiate the Pose estimate of VIO to get linear velocities and transform it to body frame and feed it as a twist input measurement to the Robot_Localization module . according to this : https://github.com/cra-ros-pkg/robot_... only acceleration is used to predict velocity, I think you'd get more accurate data if you differentiate VIO pose.

Also, are you using the VIO provided by ETH zurich ? How did you manage to compile it for Kinetic ? I've been trying to compile ROVIO for 16.04 for weeks now.

edit flag offensive delete link

Comments

2) How can I check static TF of IMU and Camera? Becasue the TF between IMU and camera are depending on REP-105, and only I can do is to set the frame. Also, yes, I use the VIO provided by ETH zurich. But I haven't used ROVIO, so I'm not sure how to complie ROVIO for Kinetic

Jacky ( 2018-04-09 03:17:43 -0600 )edit

I meant TF between IMU to base_link and Camera to base_link never mind, did you try the remove_gravitation part?

Salahuddin_Khan ( 2018-04-09 04:11:03 -0600 )edit

It could happen that a component of gravity is being reflected on one of the x,y axis as drone could tilt translating in all x,y,z directions maybe thats what is getting integrated by the Kalman filter of robot_localization and that's why the velocities are getting blown.

Salahuddin_Khan ( 2018-04-09 04:13:54 -0600 )edit

I tried remove_gravitation, but the velocities are still getting blown

Jacky ( 2018-04-09 22:58:50 -0600 )edit

add a comment

robot_localization:fusion of IMU and visual inertial odometry(VIO)

Comments

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

robot_localization:fusion of IMU and visual inertial odometry(VIO) edit

Comments

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

robot_localization:fusion of IMU and visual inertial odometry(VIO)