Revision history [back]

Thank you for the clear and detailed question.

My first suggestion is that you need to get rid of the rejection_threshold parameters. Get everything working roughly how you want, and then introduce those parameters to handle outlier rejection, if that's necessary.

Now, I see the following issues:

• You have the EKF configured so that its output frame (world_frame) is _odom_fil_. However, your visual odometry data is in the frame _/odom_ (side note: get rid of the forward slash there; tf2 won't like it). But I don't see a transform specified anywhere from _odom_fil_->_odom_. The EKF won't be able to use your VO pose data without that transform.
• You are fusing acceleration data from your IMU, but you have the IMU specified as providing data in the _base_link_ frame, but clearly it is not mounted neutrally, or your Y linear acceleration wouldn't be -9.81. A neutrally-mounted IMU should have a Z acceleration of +9.81. So you either need to fix your IMU data, or provide a transform from _base_link_ -> _imu_, and change the frame ID in your IMU data.
• You don't have a velocity reference, but you are fusing acceleration data. That's going to make for some ugliness in your velocity output (especially if your IMU has any biases), and your state estimate will definitely suffer. Your VO velocity data is in the frame _zed_camera_, so if you provide the correct transform there, you can fuse the VO velocity data.

Thank you for the clear and detailed question.

My first suggestion is that you need to get rid of the rejection_threshold parameters. Get everything working roughly how you want, and then introduce those parameters to handle outlier rejection, if that's necessary.

Now, I see the following issues:

• You have the EKF configured so that its output frame (world_frame) is _odom_fil_. odom_fil. However, your visual odometry data is in the frame _/odom_ /odom (side note: get rid of the forward slash there; tf2 won't like it). But I don't see a transform specified anywhere from _odom_fil_->_odom_. odom_fil->odom. The EKF won't be able to use your VO pose data without that transform.
• You are fusing acceleration data from your IMU, but you have the IMU specified as providing data in the _base_link_ base_link frame, but clearly it is not mounted neutrally, or your Y linear acceleration wouldn't be -9.81. A neutrally-mounted IMU should have a Z acceleration of +9.81. So you either need to fix your IMU data, or provide a transform from _base_link_ base_link -> _imu_, imu, and change the frame ID in your IMU data.
• You don't have a velocity reference, but you are fusing acceleration data. That's going to make for some ugliness in your velocity output (especially if your IMU has any biases), and your state estimate will definitely suffer. Your VO velocity data is in the frame _zed_camera_, zed_camera, so if you provide the correct transform there, you can fuse the VO velocity data.