Fusing uncontineuous global pose data

[roboticsai]

I am fusing the map pose data which is calculated from the fiducial_slam node. Here the topic being fused with ekf_node is /fiducial_pose (geometry_msgs/PoseWithCovarianceStamped). The problem i'm getting is, once the fiducial_slam don't publishes the pose data when it don't detects any fiducial/aruco markers. The robot is continusly drifting namely the odom frame is drifting with respect to time even if the robot is not moving. Here is my ekf_node params:

frequency: 10
two_d_mode: true

map_frame: map
odom_frame: odom
base_link_frame: base_footprint
world_frame: map
publish_tf: true

transform_time_offset: 0.1

smooth_lagged_data: true
dynamic_process_noise_covariance: true

odom0: base_controller/odom
odom0_config: [false, false, false,
               false, false, false,
               true,  false,  false,
               false, false, true,
               false, false, false]
# if odom0_differential is set to true x, y and yaw will be differentiated to get vx, vy and vyaw
odom0_differential: true

# Only use angular velocity from imu
imu0: /imu/data
imu0_config: [false, false, false,
              false, false, false,
              false,  false,  false,
              false, false, true,
              false, false, false]
imu0_differential: true
imu0_relative: true 

# Use x,y position and yaw orientation data for map positioning
pose0: fiducial_pose 
pose0_config: [true,  true,  false,
               false, false, true,
               false, false, false,
               false, false, false,
               false, false, false]

pose0_rejection_threshold: 2

# pose1: initialpose # External map position of robot
# pose1_config: [true,  true,  false,
#                false, false, true,
#                false, false, false,
#                false, false, false,
#                false, false, false]

# pose1_rejection_threshold: 3

# Since AMCL is more likely to fail than the beacons, you might want to treat the beacons
# as a primary source, and set pose1_differential: true
# In that case, set x', y' and yaw' to true, and the rest to false!

# pose1: /fiducial_pose # AMCL!
# pose1_config: [true,  true, false,
#                false, false, true,
#                false, false, false,
#                false, false, false,
#                false, false, false]

# pose1_rejection_threshold: 2

# poseN_rejection_threshold: (YOU MIGHT WANT THIS. If it's above the stated threshold value, the EKF will ignore the reading) (Defaults to numeric_limits<double>::max())

# [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is exposed as a configuration parameter. This matrix represents the noise we add to the total error after each prediction step. 

# The better the omnidirectional motion model matches your system, the smaller these values can be. However, if users find that a given variable is slow to converge, one approach is to increase the process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error to be larger, which will cause the filter to trust the incoming measurement more during correction. 

# The values are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. 
# Defaults to the matrix below if unspecified.

###########################################################################################################################
###########################################################################################################################

process_noise_covariance: [0.05, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0.05, 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.06, 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]

# This represents the initial value for the state estimate error covariance matrix. Setting a diagonal value (a
# variance) to a large value will result in early measurements for that variable being accepted quickly. Users should
# take care not to use large values for variables that will not be measured directly. The values are ordered as x, y,
# z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if unspecified. -->
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]

Environment: OS: ubuntu 20.04 ROS version: noetic

[ayrton04]

This should ideally be a question on answers.ros.org or Robotics Stack Exchange. Would you mind asking there? And please include a sample message from every sensor input, and maybe remove the comments from the config. Thanks!

[Wayne-xixi]

@roboticsai Hi, did u get it working?

[ayrton04]

https://robotics.stackexchange.com/questions/23149/fusing-uncontineuous-global-pose-data/23150#23150