Fusing uncontineuous global pose data [closed]

edit

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 ...