0
$\begingroup$

Rosanswers logo

I'm doing 3D state estimation with r_l and having some trouble. I have 3 sensors that I'm fusing:

  1. 2D wheel encoder based wheel odometry
  2. zed camera odom, spews both xyz and rpy. seems like rpy is a little unreliable so I'm only using xyz
  3. 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/10l0JzwfpiKgJ6QzzYa1CTwHuNBuXN8Rq/view?usp=sharing

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, 0.0, 0.0, 0.0, 0.0, 0.0, 3.14, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 3.14, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.11]


/camera_manager1/zed_node/odom:

header: 
  seq: 11550
  stamp: 
    secs: 1605734911
    nsecs: 320992919
  frame_id: "odom"
child_frame_id: "base_link"
pose: 
  pose: 
    position: 
      x: -18.1266896011
      y: -5.53277816934
      z: 0.170423537385
    orientation: 
      x: -0.0499240508418
      y: 0.0582505086796
      z: 0.908688555143
      w: 0.410365175348
  covariance: [1.350922786969022e-07, -2.6458408797225275e-08, -4.9161410942133443e-08, 4.6552733889981823e-10, -1.7202321345166638e-08, -3.6302372219410017e-09, -2.6458442548005223e-08, 2.3175346086645732e-07, -1.981402597550641e-08, -7.005017010897063e-08, -3.1487271456853705e-08, -1.804936999860729e-07, -4.91614180475608e-08, -1.9814009988294856e-08, 1.1714094227954774e-07, 5.9563269871887314e-08, 6.645198169508149e-08, 2.1804458683050143e-08, 4.6553233490342905e-10, -7.00501416872612e-08, 5.956328053002835e-08, 1.7041864452949085e-07, 3.914125912274358e-08, 2.956911160367781e-08, -1.720231779245296e-08, -3.148722882428956e-08, 6.645198169508149e-08, 3.9141266228170934e-08, 5.88976760695914e-08, 3.0582313570448605e-08, -3.630201472759609e-09, -1.8049377104034647e-07, 2.180447111754802e-08, 2.956914002538724e-08, 3.058235265029907e-08, 1.8063735751638887e-07]
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]
---

/vectornav/IMU:

header: 
  seq: 7163
  stamp: 
    secs: 1605735040
    nsecs: 521131793
  frame_id: "imu_link"
orientation: 
  x: 0.000156922101265
  y: -0.00479592956936
  z: 0.842789371954
  w: -0.538222083274
orientation_covariance: [174.49801861439306, 0.0, 0.0, 0.0, 174.49801861439306, 0.0, 0.0, 0.0, 174.49801861439306]
angular_velocity: 
  x: 0.00170283741318
  y: 0.000185212877113
  z: -0.0122508965433
angular_velocity_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
linear_acceleration: 
  x: 0.288795381784
  y: -0.0316664427519
  z: 9.87611103058
linear_acceleration_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
---

Originally posted by mugetsu on ROS Answers with karma: 195 on 2020-11-25

Post score: 0


Original comments

Comment by Tom Moore on 2020-12-09:
Hi there, please include a sample message from every sensor input.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

Without more information (see my comment about including sensor messages), my guess is that:

  1. You aren't measuring any of the 3D orientation variables from the non-IMU inputs (this makes sense, given what you said).
  2. Your IMU data isn't being respected. This is often because you don't have a transform defined from the IMU's frame to your robot's body frame.

The real issue is (2). If you want the filter to estimate your state, you have to provide a reference for every variable in the state, whether it's for the absolute dimension (e.g., RPY) or the velocities for those dimensions. If including the IMU data does nothing, then the filter is ignoring it.

BTW, your IMU config comment doesn't match your comment. You are fusing absolute orientation.

EDIT after updates:

Do you have a transform defined from base_link->imu_link anywhere? If not, you need one.

Regardless, yes, those covariances are utterly massive. The standard deviation for each angular dimension is ~13.2 radians. I'm not sure how you or you IMU driver are computing them, but the filter would definitely behave strangely (and largely ignore measurements) with covariances like that.


Originally posted by Tom Moore with karma: 13689 on 2020-12-09

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by mugetsu on 2020-12-10:
hi, thanks for taking a look. I've updated my post with the sensor input sample. It appears that the issue may be my IMU's covariance? Such large covariance makes the IMU not trusted much at all.

The IMU comment needs to be updated. I'm using fusing the orientation as a relative value.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.