2026-09-14-v1

# AMCL configuration validated against the corrected, last-ray-referenced
# /scan_merged_filtered pipeline.
#
# Follow-up bag rosbag2_2026_09_11-20_18_37 showed that the remaining error was concentrated
# in reverse arcs: the differential model cannot represent the observed lateral/curvature
# uncertainty. A controlled replay through the real C++ node, using identical scans and initial
# pose, measured:
#
#                                  all xy median       arc xy median       reverse-left arc
#   DifferentialMotionModel          6.4 cm              13.5 cm              16.9 cm
#   OmniMotionModel, alpha5=0.4       3.7 cm               4.9 cm               5.5 cm
#
# Pure-rotation accuracy was unchanged (3.7 cm versus 3.6 cm). The omni model is used here to
# let the particle cloud cover lateral slip and curvature error; it does not imply that the AGV
# is commanded to strafe.

amcl:
  ros__parameters:
    robot_model_type: hmc_amcl::OmniMotionModel

    # Replays on the follow-up bag found no benefit from reducing this to 0.05.
    alpha1: 0.10
    alpha2: 0.05
    alpha3: 0.1
    alpha4: 0.1
    # Lateral uncertainty from translation. alpha5=0.4 was the conservative point in the
    # measured plateau: it removed most reverse-arc error without increasing particle count.
    alpha5: 0.4

    global_frame_id: map
    odom_frame_id: odom
    base_frame_id: base_link
    map_topic: map_nav

    # [CHANGED] Point this at the time-corrected merged scan once
    # hmc_bringup/scan_merger is wired in (fixed_frame:=odom). The /scan in the
    # 0909 bag is stitched from a current front sweep and a 44 ms old rear sweep.
    scan_topic: /scan_merged_filtered

    initial_pose_topic: initialpose

    max_particles: 5000
    min_particles: 1000
    pf_err: 0.05

    # [CHANGED] 0.99 -> 3.0. Beluga's kld_z is a standard-normal z-SCORE, not a
    # probability (see AmclParams::kld_z, default 3.0). Passing 0.99 makes the KLD
    # bound far too loose, so the filter never adapts above min_particles.
    pf_z: 3.0

    recovery_alpha_fast: 0.0
    recovery_alpha_slow: 0.0
    resample_interval: 1

    # [ADDED] Resampling on every update with no ESS test depletes particle diversity
    # and makes the filter over-confident, which is exactly the failure mode when a
    # systematic odometry bias needs to be corrected continuously.
    selective_resampling: true

    update_min_a: 0.01
    update_min_d: 0.01

    laser_model_type: likelihood_field
    laser_likelihood_max_dist: 2.0
    laser_max_range: 30.0

    # [ADDED] There is a permanent cluster of returns at 0.65 m centred on +47 deg in
    # base_link (about 54 bins, present in >50% of scans) -- structure on the robot, not
    # in the map. laser_min_range defaults to 0.0, so those beams are currently scored
    # against the map as if they were real obstacles.
    laser_min_range: 0.8

    max_beams: 200
    z_hit: 0.9
    z_rand: 0.1
    # NOTE: z_short and z_max are read only by the beam model. LikelihoodFieldModelBase
    # uses z_hit, z_random and sigma_hit only, so the two lines below were dead
    # parameters and have been removed to avoid implying they do something.
    sigma_hit: 0.2

    tf_broadcast: true
    transform_tolerance: 0.05
    execution_policy: seq
    first_map_only: false

    set_initial_pose: true
    save_pose_rate: 0.5
    initialize_at_saved_pose: true
    saved_pose_filepath: /tmp/amcl_saved_pose
    always_reset_initial_pose: false

    global_localization_scan_matching: true
    global_localization_max_beams: 200
    global_localization_coarse_xy_step: 0.5
    global_localization_yaw_bins: 36
    global_localization_max_candidates: 8
    global_localization_refine_xy_radius: 0.25
    global_localization_refine_xy_step: 0.1
    global_localization_refine_yaw_step: 0.08726646259971647
    global_localization_xy_noise: 0.2
    global_localization_yaw_noise: 0.17453292519943295

    initial_pose.x: 0.0
    initial_pose.y: -2.0
    initial_pose.yaw: 0.0
    initial_pose.covariance_x: 0.25
    initial_pose.covariance_y: 0.25
    initial_pose.covariance_yaw: 0.0685
    initial_pose.covariance_xy: 0.0
    initial_pose.covariance_xyaw: 0.0
    initial_pose.covariance_yyaw: 0.0