1
$\begingroup$

Recently i have fused odom and imu data using ekf the filterd odom data is very good .For Vx and theta velocity the localisation is good but when i put both of the velocities 0.5 the robot misses its localisation and becomes stray. I have fused IMU and Odometry Please find the video of the problem video. Below are the AMCL parameters.

    amcl:
  ros__parameters:
    use_sim_time: False
    alpha1: 1.0
    alpha2: 0.8
    alpha3: 0.5
    alpha4: 0.3
    alpha5: 0.2
    base_frame_id: "base_footprint"
    beam_skip_distance: 0.5
    beam_skip_error_threshold: 0.9
    beam_skip_threshold: 0.3
    do_beamskip: false
    global_frame_id: "map"
    lambda_short: 0.1
    laser_likelihood_max_dist: 2.0
    laser_max_range: -1.0
    laser_min_range: -1.0
    laser_model_type: "likelihood_field"
    max_beams: 60
    max_particles: 2000
    min_particles: 500
    odom_frame_id: "odom"
    pf_err: 0.05
    pf_z: 0.99
    recovery_alpha_fast: 0.0
    recovery_alpha_slow: 0.0
    resample_interval: 1
    robot_model_type: "nav2_amcl::DifferentialMotionModel"
    save_pose_rate: 0.5
    initial_pose: [0.0, 0.0, 0.0]
    set_initial_pose: True
    sigma_hit: 0.2
    tf_broadcast: true
    transform_tolerance: 0.2
    update_min_a: 0.01
    update_min_d: 0.01
    z_hit: 0.5
    z_max: 0.05
    z_rand: 0.5
    z_short: 0.05
    scan_topic: lidar_front_right/scan

amcl_map_client:
  ros__parameters:
    use_sim_time: False

amcl_rclcpp_node:
  ros__parameters:
    use_sim_time: False

bt_navigator:
  ros__parameters:
    use_sim_time: False
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /filtered_odometry
    default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml" ####remove and check
    bt_loop_duration: 10
    default_server_timeout: 20
    enable_groot_monitoring: True
    groot_zmq_publisher_port: 1666
    groot_zmq_server_port: 1667
    plugin_lib_names:
    - nav2_compute_path_to_pose_action_bt_node
    - nav2_compute_path_through_poses_action_bt_node
    - nav2_follow_path_action_bt_node
    - nav2_back_up_action_bt_node
    - nav2_spin_action_bt_node
    - nav2_wait_action_bt_node
    - nav2_clear_costmap_service_bt_node
    - nav2_is_stuck_condition_bt_node
    - nav2_goal_reached_condition_bt_node
    - nav2_goal_updated_condition_bt_node
    - nav2_initial_pose_received_condition_bt_node
    - nav2_reinitialize_global_localization_service_bt_node
    - nav2_rate_controller_bt_node
    - nav2_distance_controller_bt_node
    - nav2_speed_controller_bt_node
    - nav2_truncate_path_action_bt_node
    - nav2_goal_updater_node_bt_node
    - nav2_recovery_node_bt_node
    - nav2_pipeline_sequence_bt_node
    - nav2_round_robin_node_bt_node
    - nav2_transform_available_condition_bt_node
    - nav2_time_expired_condition_bt_node
    - nav2_distance_traveled_condition_bt_node
    - nav2_single_trigger_bt_node
    - nav2_is_battery_low_condition_bt_node
    - nav2_navigate_through_poses_action_bt_node
    - nav2_navigate_to_pose_action_bt_node
    - nav2_remove_passed_goals_action_bt_node
    - nav2_planner_selector_bt_node
    - nav2_controller_selector_bt_node
    - nav2_goal_checker_selector_bt_node

bt_navigator_rclcpp_node:
  ros__parameters:
    use_sim_time: False

controller_server:
  ros__parameters:
    use_sim_time: true
    controller_frequency: 10.0
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001
    failure_tolerance: 10.0
    progress_checker_plugin: "progress_checker"
    goal_checker_plugins: ["general_goal_checker"]
    controller_plugins: ["FollowPath"]
    odom_topic: /filtered_odometry

    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time_allowance: 10.0

    general_goal_checker:
      stateful: True
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.3
      yaw_goal_tolerance: 0.17

    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      debug_trajectory_details: True
      min_vel_x: -0.0
      min_vel_y: 0.0
      max_vel_x: 0.8  #For adjusting speed
      max_vel_y: 0.0
      max_vel_theta: 0.8 #For adjusting angle
      min_speed_xy: 0.0
      max_speed_xy: 0.62
      min_speed_theta: 0.05
      acc_lim_x: 2.5
      acc_lim_y: 0.0
      acc_lim_theta: 4.0
      decel_lim_x: -2.5
      decel_lim_y: 0.0
      decel_lim_theta: -3.8  #-3.2
      vx_samples: 200
      vy_samples: 0 
      vtheta_samples: 40
      sim_time: 1.5
      linear_granularity: 0.05
      angular_granularity: 0.025
      transform_tolerance: 0.3
      xy_goal_tolerance: 0.3
      trans_stopped_velocity: 0.25
      short_circuit_trajectory_evaluation: True
      stateful: True
      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
      BaseObstacle.scale: 0.02
      PathAlign.scale: 32.0
      PathAlign.forward_point_distance: 0.325
      GoalAlign.scale: 24.0
      GoalAlign.forward_point_distance: 0.325
      PathDist.scale: 32.0
      GoalDist.scale: 20.0
      RotateToGoal.scale: 32.0
      RotateToGoal.slowing_factor: 5.0
      RotateToGoal.lookahead_time: -1.0
      
controller_server_rclcpp_node:
  ros__parameters:
    use_sim_time: False

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 10.0
      publish_frequency: 5.0
      global_frame: map
      robot_base_frame: base_footprint
      use_sim_time: False
      rolling_window: true
      width: 5
      height: 5
      resolution: 0.1
      robot_radius: 0.42
      plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"]
      filters: ["keepout_filter", "inflation_layer"]
      keepout_filter:
        plugin: "nav2_costmap_2d::KeepoutFilter"
        enabled: True
        filter_info_topic: "/costmap_filter_info"
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 15.0
        inflation_radius: 0.2
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        inflation_radius: 0.1     #0.2---1
        observation_sources: lidar_front_right/scan lidar_rear_left/scan
        lidar_front_right/scan:
          topic: /lidar_front_right/scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
        lidar_rear_left/scan:
          topic: /lidar_rear_left/scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: True
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: lidar_front_right/scan lidar_rear_left/scan
        lidar_front_right/scan:
          topic: /lidar_front_right/scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 5.0
          raytrace_min_range: 0.0
          obstacle_max_range: 5.0
          obstacle_min_range: 0.0
        lidar_rear_left/scan:
          topic: /lidar_rear_left/scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 6.0
          raytrace_min_range: 0.0
          obstacle_max_range: 5.0
          obstacle_min_range: 0.0
      static_layer:
        map_subscribe_transient_local: True
      always_send_full_costmap: True

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 10.0
      publish_frequency: 5.0
      global_frame: map
      robot_base_frame: base_footprint
      use_sim_time: false
      robot_radius: 0.42
      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer"]
      filters: ["keepout_filter", "inflation_layer"]
      keepout_filter:
        plugin: "nav2_costmap_2d::KeepoutFilter"
        enabled: True
        filter_info_topic: "/costmap_filter_info"
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        inflation_radius: 0.6
        observation_sources: lidar_front_right/scan lidar_rear_left/scan
        lidar_front_right/scan:
          topic: /lidar_front_right/scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 6.0
          raytrace_min_range: 0.0
          obstacle_max_range: 5.0
          obstacle_min_range: 0.0
        lidar_rear_left/scan:
          topic: /lidar_rear_left/scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 6.0
          raytrace_min_range: 0.0
          obstacle_max_range: 5.0
          obstacle_min_range: 0.0
      voxel_layer:   # Move this section outside of ros__parameters
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: True
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: lidar_front_right/scan lidar_rear_left/scan
        lidar_front_right/scan:
          topic: /lidar_front_right/scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 5.0
          raytrace_min_range: 0.0
          obstacle_max_range: 5.0
          obstacle_min_range: 0.0
        lidar_rear_left/scan:
          topic: /lidar_rear_left/scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 5.0
          raytrace_min_range: 0.0
          obstacle_max_range: 5.0
          obstacle_min_range: 0.0
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 25.0
        inflation_radius: 0.6
      always_send_full_costmap: True
  global_costmap_client:
    ros__parameters:
      use_sim_time: False
  global_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: False

map_server:
  ros__parameters:
    use_sim_time: False
    yaml_filename: "map_17_10_24.yaml"

map_saver:
  ros__parameters:
    use_sim_time: False
    save_map_timeout: 5000  #foxy 5000
    free_thresh_default: 0.25
    occupied_thresh_default: 0.65
    map_subscribe_transient_local: True

planner_server:
  ros__parameters:
    expected_planner_frequency: 1.0
    use_sim_time: False
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      tolerance: 1.0  #foxy 1.0
      use_astar: true  #foxy true
      allow_unknown: true

planner_server_rclcpp_node:
  ros__parameters:
    use_sim_time: False

recoveries_server:
  ros__parameters:
    costmap_topic: local_costmap/costmap_raw
    footprint_topic: local_costmap/published_footprint
    cycle_frequency: 1.0
    recovery_plugins: ["spin", "backup", "wait"]
    spin:
      plugin: "nav2_recoveries/Spin"
    backup:
      plugin: "nav2_recoveries/BackUp"
    wait:
      plugin: "nav2_recoveries/Wait"
    global_frame: odom
    robot_base_frame: base_footprint
    transform_timeout: 0.1
    use_sim_time: false
    simulate_ahead_time: 2.0
    max_rotational_vel: 0.3  #foxy 3.0
    min_rotational_vel: 0.05
    rotational_acc_lim: 1.2   #foxy 1.2

robot_state_publisher:
  ros__parameters:
    use_sim_time: False

waypoint_follower:
  ros__parameters:
    loop_rate: 2000  ####20
    stop_on_failure: false
    waypoint_task_executor_plugin: "wait_at_waypoint"   
    wait_at_waypoint:
      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
      enabled: True
      waypoint_pause_duration: 200
smoother_server:
  ros__parameters:
    # General parameters
    update_rate: 20.0

    # Trajectory smoothing parameters
    smoother_type: "velocity_smoother"
    velocity_smoother:
      type: "nav2_smoother::VelocitySmoother"
      lookahead_time: 0.5
      samples: 10
      min_samples: 5
      max_samples: 20
      alpha: 0.8
      beta: 0.2
      max_velocity_xy: 0.5
      max_velocity_theta: 0.5
      smoothing_frequency: 80.0

    # Other parameters
    motion_model_for_search: "DifferentialDrive"
    pose_distance: 0.25
    publish_predicted_path: false
    publish_filtered_path: false

realsense_node:
  ros__parameters:
    topics:
      color:
        image: "/camera/color/image_raw"
        camera_info: "/camera/color/camera_info"
      depth:
        image: "/camera/depth/image_rect_raw"
      infra1:
        image: "/camera/infra1/image_rect_raw"
      infra2:
        image: "/camera/infra2/image_rect_raw"
    depth_scale_meters: 0.001
behavior_server:
  ros__parameters:
    bond_disable_heartbeat_timeout: true
    backup.plugin: nav2_behaviors/BackUp
    behavior_plugins:
      - spin
      - backup
      - drive_on_heading
      - wait
    costmap_topic: local_costmap/costmap_raw
    cycle_frequency: 20.0
    drive_on_heading.plugin: nav2_behaviors/DriveOnHeading
    footprint_topic: local_costmap/published_footprint
    global_frame: odom
    max_rotational_vel: 0.1
    min_rotational_vel: 0.05
    robot_base_frame: base_link
    rotational_acc_lim: 2.0
    simulate_ahead_time: 2.0
    spin.plugin: nav2_behaviors/Spin
    transform_tolerance: 0.5
    use_sim_time: false
    wait.plugin: nav2_behaviors/Wait
    
velocity_smoother:
  ros__parameters:
    bond_disable_heartbeat_timeout: true
    feedback: OPEN_LOOP
    odom_duration: 0.1
    odom_topic: /filtered_odometry
    scale_velocities: true
    smoothing_frequency: 150.0
    use_sim_time: False
    velocity_timeout: 1.0

  **EKF node**

ekf_filter_node:
    ros__parameters:

        use_sim_time: false
        frequency: 30.0
        sensor_timeout: 0.1
        two_d_mode: true
        transform_time_offset: 0.0
        transform_timeout: 0.0
        print_diagnostics: true
        debug: false
        debug_out_file: /path/to/debug/file.txt
        publish_tf: false
        publish_acceleration: false
        reset_on_time_jump: false


        map_frame: map                  
        odom_frame: /odom                
        base_link_frame: /base_footprint 
        world_frame: /odom                
        odom0: /odom
        odom0_config: [true, true, true,
                       false, false, true,
                       true,  true,  true,
                       false, false, false,
                       false, false, false]

        odom0_queue_size: 2
        odom0_nodelay: false
        odom0_differential: false
        odom0_relative: true
        odom0_pose_rejection_threshold: 5.0
        odom0_twist_rejection_threshold: 1.0

        imu0: /imu_data
        imu0_config: [false, false, false,
                      true,  true,  false,
                      true, true, true,
                      true,  true,  true,
                      true,  true,  true]
        imu0_nodelay: false
        imu0_differential: false
        imu0_relative: true
        imu0_queue_size: 7
        imu0_pose_rejection_threshold: 0.8                
        imu0_twist_rejection_threshold: 0.8                
        imu0_linear_acceleration_rejection_threshold: 0.8  
        imu0_remove_gravitational_acceleration: true
        use_control: false
        stamped_control: false
        control_timeout: 0.2
        control_config: [true, false, false, false, false, true]
        acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
        deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
        acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
        deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]
        process_noise_covariance: [0.05,   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.05,   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.06,   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.03,   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.03,   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.06,   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.025,   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.025,   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.04,   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.01,   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.01,   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.02,   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.01,   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.01,   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.015]
        initial_estimate_covariance: [1e-9,   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,    1e-9,   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,    1e-9,   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,    1e-9,   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,    1e-9,   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,    1e-9,   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,    1e-9,   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,    1e-9,   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,    1e-9,   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,    1e-9,    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,     1e-9,    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,     1e-9,    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,     1e-9,   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,    1e-9,   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,    1e-9]

edit: added EFK node

imu_data (topic) output below

    header:
  stamp:
    sec: 1732770428
    nanosec: 327322667
  frame_id: imu_link
orientation:
  x: -0.03497527912259102
  y: 0.07251419126987457
  z: -0.8936092257499695
  w: 0.44167736172676086
orientation_covariance:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
angular_velocity:
  x: 0.0625
  y: 0.0625
  z: 0.0625
angular_velocity_covariance:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
linear_acceleration:
  x: -0.10999999940395355
  y: -0.009999999776482582
  z: -0.07000000029802322
linear_acceleration_covariance:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
---

Below echo from /odom

header:
  stamp:
    sec: 1732770536
    nanosec: 999883669
  frame_id: /odom
child_frame_id: base_footprint
pose:
  pose:
    position:
      x: 0.007230392816932821
      y: 0.0007949371304678404
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 0.03194644236682118
      w: 0.9994895821468592
  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
twist:
  twist:
    linear:
      x: 5.890486225480862e-05
      y: 0.0
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: -0.00021381075228605668
  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
---
Improve this question
$\endgroup$
4
  • $\begingroup$ I don't see any config for your EKF in your parameters, unless you're using a package with which I am not familiar. You should also include sample messages from each sensor input, both when things are working and when they are not. $\endgroup$
    – automatom
    Commented Nov 6 at 14:52
  • $\begingroup$ Thanks for addressing my issue @automatom Here are my EKF parameters. When you say i shoudl include sample messages from each sensor input can you give me and example $\endgroup$
    – sidharth bits
    Commented Nov 13 at 4:45
  • $\begingroup$ You have two sensor inputs: /odom and /imu_data. If you can do a ros2 topic echo on each of those topics and paste one sample message from each of them in the question, that would be great. $\endgroup$
    – automatom
    Commented Nov 14 at 16:43
  • $\begingroup$ @automatom i have edited the question please find the details on /odom and /imu_data you have asked for .. thank you in advance $\endgroup$
    – sidharth bits
    Commented Nov 28 at 5:11

1 Answer 1

Reset to default
0
$\begingroup$

That just looks like lag in your state estimate. Your rviz video shows that you're viewing everything in the map frame. You are using amcl to generate the map->odom transform and the EKF to generate the odom->base_link transform, so it's hard to say what the source of your lag is. The only reason I'd lean towards the issue being in amcl (or the data you are providing to it) is the fact that any node producing the map->odom transform should be looking up the odom->base_link transform at every cycle, so no matter how lagged the EKF is, the robot should remain well localized when you visualize the map frame.

In any case, I can't speak to your amcl config, but for the EKF, I'd recommend the following changes:

  1. You have two_d_mode turned on. This tells the filter that your robot is operating in a planar environment, and it should ignore any measurements for 3D space (roll, pitch, Z, and their respective velocities and accelerations). But you are attempting to fuse Z position and Z velocity from your odom input, and you are fusing Z acceleration from your IMU. This doesn't really affect anything, but it can create confusion.
  2. You are also fusing X velocity, Y velocity, and Z velocity from your IMU, but IMUs don't produce those quantities, and the sensor_msgs/Imu message doesn't have fields for them.
  3. You are attempting to fuse linear accelerations. I'd remove that to start until everything else is working.
  4. I would fuse linear and angular velocity from your /odom topic, rather than position.
  5. You have leading slashes in your frame_id values for the EKF and in your odom message. tf2 will ignore those. Not critical, but worth removing.
  6. Your IMU appears to have been reporting exactly 0.0625 rad/sec in all three dimensions in your sample message. That seems very strange.
  7. Your imu_data message has a frame_id of imu_link. Is something publishing the base_footprint->imu_link transform?
  8. Have you done any analysis of your wheel odometry and IMU data to determine if there is lag in it? Are you making sure the timestamps are correct?

If it were me, I'd make sure that all my topics are being correctly subscribed to by the relevant nodes. I'd also make sure all my sensor data is absolutely correct and not lagged. To rule out the EKF being the source of lag (or to possibly verify that it's the odometry data at fault), I would visualise that movement in rviz in the odom frame (turn the map off), and persist the laser scans by making the Decay Time in the rviz display something like 10 seconds. As you rotate and translate, you should not see a huge amount of "smearing" of the laser scan. It should remain mostly in place, with only a small amount of deviation as you make that motion. Obviously, over time, it will deviate more, but for one short motion, it should be relatively stable. You should also make sure that amcl is properly subscribed to all the topics you'd expect.

Improve this answer
$\endgroup$

Your Answer

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

Not the answer you're looking for? Browse other questions tagged or ask your own question.