# FusionCore config for TurtleBot3 Waffle (Gazebo Harmonic)
#
# Platform:  TurtleBot3 Waffle (simulated)
# IMU:       MPU9250 (9-axis, magnetometer unreliable in sim)
# GPS:       None (indoor / simulation)
# LiDAR:     HLS-LFCD2 via /scan (used by icp_odometry, not fused here directly)
# encoder2:  rtabmap icp_odometry output on /rtabmap/icp_odometry
#
# Architecture: FusionCore (wheels + IMU) provides the odom frame.
# icp_odometry uses that odom frame as its initial scan-matching guess
# (guess_frame_id: odom). The ICP output feeds back into FusionCore
# as a second velocity source (encoder2). Each tightens the other.

fusioncore:
  ros__parameters:
    base_frame: base_footprint
    odom_frame: odom
    publish_rate: 100.0
    publish.force_2d: true

    # Gazebo Harmonic prefixes sensor frames with the model name (waffle/imu_link/tb3_imu).
    # Override to the TF frame that robot_state_publisher actually publishes.
    imu.frame_id: "imu_link"

    # MPU9250: magnetometer disabled (unreliable in sim / near motors)
    imu.has_magnetometer: false
    # Simulation-tuned noise values for Gazebo Harmonic MPU9250 plugin.
    # Real MPU9250 hardware: gyro ~0.005 rad/s, accel ~0.1 m/s2.
    # Gazebo injects lower noise than the real sensor, so tighter values
    # reduce unnecessary filter uncertainty in sim without affecting real-hardware users
    # (who should revert to the hardware spec values above).
    imu.gyro_noise: 0.002             # rad/s  (Gazebo sim-tuned; real MPU9250: 0.005)
    imu.accel_noise: 0.02             # m/s2   (Gazebo sim-tuned; real MPU9250: 0.1)
    imu.remove_gravitational_acceleration: false

    # Wheel odometry noise (TB3 differential drive, simulated)
    encoder.vel_noise: 0.05           # m/s
    encoder.yaw_noise: 0.02           # rad/s

    # ICP odometry as second velocity source
    encoder2.topic: "/rtabmap/icp_odometry"

    outlier_rejection: true
    outlier_threshold_imu: 15.09
    outlier_threshold_enc: 11.34

    adaptive.imu: true
    adaptive.encoder: true
    adaptive.window: 50
    adaptive.alpha: 0.01

    zupt.enabled: true
    zupt.velocity_threshold: 0.08     # m/s: slightly loose for ICP jitter
    zupt.angular_threshold: 0.05      # rad/s
    zupt.noise_sigma: 0.01

    ukf.q_position: 0.01
    ukf.q_orientation: 1.0e-9
    ukf.q_velocity: 0.1
    ukf.q_angular_vel: 0.1
    ukf.q_acceleration: 1.0
    ukf.q_gyro_bias: 1.0e-5
    ukf.q_accel_bias: 1.0e-5