From 76f1f93c6a34643de8ed199ca38666145f1c22df Mon Sep 17 00:00:00 2001
From: migueldm <midemig@gmail.com>
Date: Fri, 13 Mar 2026 12:05:51 +0100
Subject: [PATCH] Port from rolling to jazzy

---
 .../easynav_fusion_localizer/CMakeLists.txt   |   4 +
 .../config/example_params.yaml                |   2 +-
 .../config/example_params_summit_dual.yaml    | 280 ++++++++++++++++++
 .../example_params_summit_dual_gazebo.yaml    | 280 ++++++++++++++++++
 .../FusionLocalizer.hpp                       |  25 +-
 .../easynav_fusion_localizer/ukf_wrapper.hpp  |  15 +-
 .../easynav_fusion_localizer/maps/empty.png   | Bin 0 -> 4849 bytes
 .../easynav_fusion_localizer/maps/empty.yaml  |   7 +
 .../easynav_fusion_localizer/maps/square.png  | Bin 0 -> 11002 bytes
 .../easynav_fusion_localizer/maps/square.yaml |   7 +
 .../easynav_fusion_localizer/package.xml      |   2 +-
 .../FusionLocalizer.cpp                       | 252 +++++++++++-----
 .../easynav_fusion_localizer/ukf_wrapper.cpp  |  62 ++--
 13 files changed, 833 insertions(+), 103 deletions(-)
 create mode 100644 localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml
 create mode 100644 localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml
 create mode 100644 localizers/easynav_fusion_localizer/maps/empty.png
 create mode 100644 localizers/easynav_fusion_localizer/maps/empty.yaml
 create mode 100644 localizers/easynav_fusion_localizer/maps/square.png
 create mode 100644 localizers/easynav_fusion_localizer/maps/square.yaml

diff --git a/localizers/easynav_fusion_localizer/CMakeLists.txt b/localizers/easynav_fusion_localizer/CMakeLists.txt
index 4db3bed8..94db9550 100644
--- a/localizers/easynav_fusion_localizer/CMakeLists.txt
+++ b/localizers/easynav_fusion_localizer/CMakeLists.txt
@@ -72,6 +72,10 @@ install(TARGETS
   RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
 
+install(DIRECTORY maps/
+  DESTINATION share/${PROJECT_NAME}/maps
+)
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   set(ament_cmake_copyright_FOUND TRUE)
diff --git a/localizers/easynav_fusion_localizer/config/example_params.yaml b/localizers/easynav_fusion_localizer/config/example_params.yaml
index 9c47f30b..905be4e5 100644
--- a/localizers/easynav_fusion_localizer/config/example_params.yaml
+++ b/localizers/easynav_fusion_localizer/config/example_params.yaml
@@ -25,7 +25,7 @@ localizer_node:
       latitude_origin: 40.2834931
       longitude_origin: -3.8207253999999997
       altitude_origin: 746.184
-      local_filter:
+      global_filter:
         two_d_mode: true
         publish_tf: true
 
diff --git a/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml b/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml
new file mode 100644
index 00000000..40b6d005
--- /dev/null
+++ b/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml
@@ -0,0 +1,280 @@
+controller_node:
+  ros__parameters:
+    use_sim_time: true
+    controller_types: [serest]
+    dummy:
+      rt_freq: 30.0
+      plugin: easynav_controller/DummyController
+      cycle_time_nort: 0.01
+      cycle_time_rt: 0.0001
+    mppi:
+      rt_freq: 30.0 
+      plugin: easynav_mppi_controller/MPPIController
+      num_samples: 200
+      horizon_steps: 5
+      dt: 0.15
+      lambda: 1.0      
+      fov: 1.57
+      max_linear_velocity: 1.5
+      max_angular_velocity: 1.0
+      max_linear_acceleration: 0.5
+      max_angular_acceleration: 0.5
+      safety_radius: 0.3
+    serest:
+      rt_freq: 30.0 
+      plugin: easynav_serest_controller/SerestController
+      allow_reverse: true
+      max_linear_speed: 0.8
+      max_angular_speed: 1.2
+      v_progress_min: 0.08        # 8 cm/s de crucero mínimo cuando alineado
+      k_s_share_max: 0.5          # succión lateral no cancela >50% del avance
+      k_theta: 2.5                # sube un poco
+      k_y: 1.5
+      goal_pos_tol: 0.1          # 7 cm
+      goal_yaw_tol_deg: 6.0       # 6 grados
+      slow_radius: 0.60
+      slow_min_speed: 0.03
+      final_align_k: 2.0
+      final_align_wmax: 0.6
+      corner_guard_enable: true
+      corner_gain_ey: 1.8
+      corner_gain_eth: 0.7
+      corner_gain_kappa: 0.4
+      corner_min_alpha: 0.35
+      corner_boost_omega: 1.0
+      a_lat_soft: 0.9
+      apex_ey_des: 0.05
+
+localizer_node:
+  ros__parameters:
+    use_sim_time: true
+    localizer_types: [Ukf]
+    dummy:
+      rt_freq: 30.0
+      plugin: easynav_localizer/DummyLocalizer
+      cycle_time_nort: 0.01
+      cycle_time_rt: 0.001
+    Ukf:
+      rt_freq: 50.0
+      freq: 5.0
+      reseed_freq: 0.1
+      plugin: easynav_fusion_localizer/FusionLocalizer
+      latitude_origin: 40.2834931
+      longitude_origin: -3.8207253999999997
+      altitude_origin: 746.184
+      local_filter:
+        two_d_mode: true
+        publish_tf: true
+
+        # --- Reference Frames ---
+        map_frame: map
+        odom_frame: odom
+        base_link_frame: base_link
+        world_frame: odom
+
+        dynamic_process_noise_covariance: true
+
+        #--- INPUT 2: Wheel Odometry ---
+        odom0: /robotnik_base_control/odom
+        odom0_config:
+          [true,  true, false,
+          false, false, true,
+          true,  true, false,
+          false, false, true,
+          false, false, false]
+        odom0_differential: false
+
+        # --- INPUT 3: IMU ---
+        # imu0: /imu/data
+        # imu0_config:
+        #   [false, false, false,
+        #   false, false, false,
+        #   false, false, false,
+        #   false, false, true,
+        #   true, true, true]
+        # imu0_differential: false
+        # imu0_remove_gravitational_acceleration: true
+
+        # twist0: /gps/fix_velocity
+        # twist0_config:
+        #   [false, false, false,
+        #   false, false, false,
+        #   true,  false,  false,
+        #   false, false, false,
+        #   false, false, false]
+        # twist0_differential: false
+
+        # --- Process Variances ---
+        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]
+      global_filter:
+        two_d_mode: true
+        publish_tf: true
+        debug: true
+        debug_out_file: /tmp/ukf_global_debug.txt
+        transform_timeout: 0.1
+        smooth_lagged_data: true
+        # Defines the time window in seconds to keep past states in memory.
+        # history_length: 5.0
+        dynamic_process_noise_covariance: true
+
+        # --- Reference Frames ---
+        map_frame: map
+        odom_frame: odom
+        base_link_frame: base_link
+        world_frame: map
+
+        # --- INPUT 1: GPS Odometry ---
+        gps0: /gps/fix
+        gps0_config:
+          [true,  true,  false,
+          false, false, false,
+          false, false, false,
+          false, false, false,
+          false, false, false]
+        gps0_differential: false
+
+        # --- INPUT 2: Wheel Odometry ---
+        odom0: /robotnik_base_control/odom
+        odom0_config:
+          [false, false, false,
+          false, false, false,
+          true,  true,  false,
+          false, false, true,
+          false, false, false]
+        odom0_differential: false
+
+        # --- INPUT 3: IMU ---
+        imu0: /imu/data
+        imu0_config:
+          [false, false, false,
+          false, false, false,
+          false, false, false,
+          false, false, true,
+          false, false, false]
+        imu0_differential: false
+        imu0_remove_gravitational_acceleration: true
+
+        # twist0: /gps/fix_velocity
+        # twist0_config:
+        #   [false, false, false,
+        #   false, false, false,
+        #   true,  true,  false,
+        #   false, false, false,
+        #   false, false, false]
+        # twist0_differential: false
+
+        # --- Process Variances ---
+        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: [
+          5.0, 5.0, 1e-3,   1e-3, 1e-3, 1.0,
+          0.5,  0.5,  1e-3,   1e-3, 1e-3, 0.1,
+          0.1,  0.1,  0.1
+        ]
+
+
+maps_manager_node:
+  ros__parameters:
+    use_sim_time: true
+    map_types: [costmap]
+    routes:
+      freq: 10.0
+      plugin: easynav_routes_maps_manager/RoutesMapsManager
+      filters: [routes_costmap]
+      package: easynav_fusion_localizer
+      map_path_file: routes/simple_square.yaml
+      routes_costmap:
+        plugin: easynav_routes_maps_manager/RoutesCostmapFilter
+        min_cost: 50
+        route_width: 0.5
+      cycle_time_nort: 0.001
+      cycle_time_rt: 0.001
+    costmap:
+      freq: 10.0
+      plugin: easynav_costmap_maps_manager/CostmapMapsManager
+      package: easynav_fusion_localizer
+      map_path_file: maps/square.yaml
+      filters: [obstacles, inflation]
+      obstacles:
+        plugin: easynav_costmap_maps_manager/CostmapMapsManager/ObstaclesFilter
+      inflation:
+        plugin: easynav_costmap_maps_manager/CostmapMapsManager/InflationFilter
+        inflation_radius: 1.3
+        inscribed_radius: 0.25
+        cost_scaling_factor: 3.0
+    dummy:
+      freq: 10.0
+      plugin: easynav_maps_manager/DummyMapsManager
+      cycle_time_nort: 0.1
+      cycle_time_rt: 0.0001
+
+planner_node:
+  ros__parameters:
+    use_sim_time: true
+    planner_types: [costmap]
+    dummy:
+      freq: 1.0
+      plugin: easynav_planner/DummyPlanner
+      cycle_time_nort: 0.2
+      cycle_time_rt: 0.0001
+    costmap:
+      freq: 10.0
+      plugin: easynav_costmap_planner/CostmapPlanner
+      cost_factor: 10.0
+      continuous_replan: true
+    simple:
+      freq: 0.5
+      plugin: easynav_simple_planner/SimplePlanner
+      robot_radius: 0.3
+
+sensors_node:
+  ros__parameters:
+    use_sim_time: true
+    forget_time: 0.5
+    sensors: [imu, gps]
+    perception_default_frame: odom
+    imu:
+      topic: /imu/data
+      type: sensor_msgs/msg/Imu
+    gps:
+      topic: /gps/fix
+      type: sensor_msgs/msg/NavSatFix
+    laser1:
+      topic: /front_laser/points
+      type: sensor_msgs/msg/PointCloud2
+      group: points
+    
+
+system_node:
+  ros__parameters:
+    use_sim_time: true
+    position_tolerance: 0.75
+    angle_tolerance: 0.25
diff --git a/localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml b/localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml
new file mode 100644
index 00000000..b7afad57
--- /dev/null
+++ b/localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml
@@ -0,0 +1,280 @@
+controller_node:
+  ros__parameters:
+    use_sim_time: true
+    controller_types: [serest]
+    dummy:
+      rt_freq: 30.0
+      plugin: easynav_controller/DummyController
+      cycle_time_nort: 0.01
+      cycle_time_rt: 0.0001
+    mppi:
+      rt_freq: 30.0 
+      plugin: easynav_mppi_controller/MPPIController
+      num_samples: 200
+      horizon_steps: 5
+      dt: 0.15
+      lambda: 1.0      
+      fov: 1.57
+      max_linear_velocity: 1.5
+      max_angular_velocity: 1.0
+      max_linear_acceleration: 0.5
+      max_angular_acceleration: 0.5
+      safety_radius: 0.3
+    serest:
+      rt_freq: 30.0 
+      plugin: easynav_serest_controller/SerestController
+      allow_reverse: true
+      max_linear_speed: 0.8
+      max_angular_speed: 1.2
+      v_progress_min: 0.08        # 8 cm/s de crucero mínimo cuando alineado
+      k_s_share_max: 0.5          # succión lateral no cancela >50% del avance
+      k_theta: 2.5                # sube un poco
+      k_y: 1.5
+      goal_pos_tol: 0.1          # 7 cm
+      goal_yaw_tol_deg: 6.0       # 6 grados
+      slow_radius: 0.60
+      slow_min_speed: 0.03
+      final_align_k: 2.0
+      final_align_wmax: 0.6
+      corner_guard_enable: true
+      corner_gain_ey: 1.8
+      corner_gain_eth: 0.7
+      corner_gain_kappa: 0.4
+      corner_min_alpha: 0.35
+      corner_boost_omega: 1.0
+      a_lat_soft: 0.9
+      apex_ey_des: 0.05
+
+localizer_node:
+  ros__parameters:
+    use_sim_time: true
+    localizer_types: [Ukf]
+    dummy:
+      rt_freq: 30.0
+      plugin: easynav_localizer/DummyLocalizer
+      cycle_time_nort: 0.01
+      cycle_time_rt: 0.001
+    Ukf:
+      rt_freq: 50.0
+      freq: 5.0
+      reseed_freq: 0.1
+      plugin: easynav_fusion_localizer/FusionLocalizer
+      latitude_origin: 40.284796759605115
+      longitude_origin: -3.8214099132625017
+      altitude_origin: 669.9730248888955
+      local_filter:
+        two_d_mode: true
+        publish_tf: false
+
+        # --- Reference Frames ---
+        map_frame: map
+        odom_frame: odom
+        base_link_frame: base_link
+        world_frame: odom
+
+        dynamic_process_noise_covariance: true
+
+        #--- INPUT 2: Wheel Odometry ---
+        odom0: /robotnik_base_control/odom
+        odom0_config:
+          [true,  true, false,
+          false, false, true,
+          true,  true, false,
+          false, false, true,
+          false, false, false]
+        odom0_differential: false
+
+        # --- INPUT 3: IMU ---
+        # imu0: /imu/data
+        # imu0_config:
+        #   [false, false, false,
+        #   false, false, false,
+        #   false, false, false,
+        #   false, false, true,
+        #   true, true, true]
+        # imu0_differential: false
+        # imu0_remove_gravitational_acceleration: true
+
+        # twist0: /gps/fix_velocity
+        # twist0_config:
+        #   [false, false, false,
+        #   false, false, false,
+        #   true,  false,  false,
+        #   false, false, false,
+        #   false, false, false]
+        # twist0_differential: false
+
+        # --- Process Variances ---
+        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]
+      global_filter:
+        two_d_mode: true
+        publish_tf: true
+        debug: true
+        debug_out_file: /tmp/ukf_global_debug.txt
+        transform_timeout: 0.1
+        smooth_lagged_data: true
+        # Defines the time window in seconds to keep past states in memory.
+        # history_length: 5.0
+        dynamic_process_noise_covariance: true
+
+        # --- Reference Frames ---
+        map_frame: map
+        odom_frame: odom
+        base_link_frame: base_link
+        world_frame: map
+
+        # --- INPUT 1: GPS Odometry ---
+        gps0: /gps/fix
+        gps0_config:
+          [true,  true,  false,
+          false, false, false,
+          false, false, false,
+          false, false, false,
+          false, false, false]
+        gps0_differential: false
+
+        # --- INPUT 2: Wheel Odometry ---
+        odom0: /robotnik_base_control/odom
+        odom0_config:
+          [false, false, false,
+          false, false, false,
+          true,  true,  false,
+          false, false, true,
+          false, false, false]
+        odom0_differential: false
+
+        # --- INPUT 3: IMU ---
+        imu0: /imu/data
+        imu0_config:
+          [false, false, false,
+          false, false, false,
+          false, false, false,
+          false, false, true,
+          false, false, false]
+        imu0_differential: false
+        imu0_remove_gravitational_acceleration: true
+
+        # twist0: /gps/fix_velocity
+        # twist0_config:
+        #   [false, false, false,
+        #   false, false, false,
+        #   true,  true,  false,
+        #   false, false, false,
+        #   false, false, false]
+        # twist0_differential: false
+
+        # --- Process Variances ---
+        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: [
+          5.0, 5.0, 1e-3,   1e-3, 1e-3, 1.0,
+          0.5,  0.5,  1e-3,   1e-3, 1e-3, 0.1,
+          0.1,  0.1,  0.1
+        ]
+
+
+maps_manager_node:
+  ros__parameters:
+    use_sim_time: true
+    map_types: [costmap, routes]
+    routes:
+      freq: 10.0
+      plugin: easynav_routes_maps_manager/RoutesMapsManager
+      filters: [routes_costmap]
+      package: easynav_fusion_localizer
+      map_path_file: routes/simple_square.yaml
+      routes_costmap:
+        plugin: easynav_routes_maps_manager/RoutesCostmapFilter
+        min_cost: 50
+        route_width: 0.5
+      cycle_time_nort: 0.001
+      cycle_time_rt: 0.001
+    costmap:
+      freq: 10.0
+      plugin: easynav_costmap_maps_manager/CostmapMapsManager
+      package: easynav_fusion_localizer
+      map_path_file: maps/empty.yaml
+      filters: [obstacles, inflation]
+      obstacles:
+        plugin: easynav_costmap_maps_manager/CostmapMapsManager/ObstaclesFilter
+      inflation:
+        plugin: easynav_costmap_maps_manager/CostmapMapsManager/InflationFilter
+        inflation_radius: 1.3
+        inscribed_radius: 0.25
+        cost_scaling_factor: 3.0
+    dummy:
+      freq: 10.0
+      plugin: easynav_maps_manager/DummyMapsManager
+      cycle_time_nort: 0.1
+      cycle_time_rt: 0.0001
+
+planner_node:
+  ros__parameters:
+    use_sim_time: true
+    planner_types: [costmap]
+    dummy:
+      freq: 1.0
+      plugin: easynav_planner/DummyPlanner
+      cycle_time_nort: 0.2
+      cycle_time_rt: 0.0001
+    costmap:
+      freq: 10.0
+      plugin: easynav_costmap_planner/CostmapPlanner
+      cost_factor: 10.0
+      continuous_replan: true
+    simple:
+      freq: 0.5
+      plugin: easynav_simple_planner/SimplePlanner
+      robot_radius: 0.3
+
+sensors_node:
+  ros__parameters:
+    use_sim_time: true
+    forget_time: 0.5
+    sensors: [imu, gps]
+    perception_default_frame: odom
+    imu:
+      topic: /imu/data
+      type: sensor_msgs/msg/Imu
+    gps:
+      topic: /gps/fix
+      type: sensor_msgs/msg/NavSatFix
+    laser1:
+      topic: /front_laser/points
+      type: sensor_msgs/msg/PointCloud2
+      group: points
+    
+
+system_node:
+  ros__parameters:
+    use_sim_time: true
+    position_tolerance: 0.75
+    angle_tolerance: 0.25
diff --git a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp
index c29ce6e9..1f49c3f8 100644
--- a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp
+++ b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp
@@ -1,17 +1,21 @@
 #pragma once
 
 #include <memory>
+#include <vector>
 
 #include "easynav_core/LocalizerMethodBase.hpp"
-#include "easynav_fusion_localizer/ukf_wrapper.hpp" // Tu wrapper refactorizado
+#include "easynav_fusion_localizer/ukf_wrapper.hpp"
 
 #include "sensor_msgs/msg/imu.hpp"
 #include "sensor_msgs/msg/nav_sat_fix.hpp"
 #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+#include "nav_msgs/msg/odometry.hpp"
 
 namespace easynav
 {
 
+constexpr double kNoOrientationCovariance = 1e5;
+
 /**
  * @class FusionLocalizer
  * @brief Plugin de localización para EasyNav que integra el UKF de
@@ -51,14 +55,20 @@ class FusionLocalizer : public easynav::LocalizerMethodBase
   void update(NavState & nav_state) override;
 
 private:
-  std::unique_ptr<robot_localization::UkfWrapper> ukf_wrapper_;
+  std::unique_ptr<robot_localization::UkfWrapper> ukf_local_{nullptr};
+  std::unique_ptr<robot_localization::UkfWrapper> ukf_global_{nullptr};
+
+  bool has_global_filter_{false};
+  bool has_local_filter_{false};
 
-  int n_imu_sensors_{0};
   int n_gps_sensors_{0};
 
   geometry_msgs::msg::PoseWithCovarianceStamped
   navsatfix_to_pose(const sensor_msgs::msg::NavSatFix & navsat_msg);
 
+  sensor_msgs::msg::NavSatFix
+  odom_to_navsatfix(const nav_msgs::msg::Odometry & odom_msg);
+
   double latitude_origin_{0.0};
   double longitude_origin_{0.0};
   double altitude_origin_{0.0};
@@ -66,8 +76,15 @@ class FusionLocalizer : public easynav::LocalizerMethodBase
   double UTM_origin_y_{0.0};
   double UTM_origin_z_{0.0};
   std::string UTM_zone_;
+  int UTM_zone_number_{0};
+  bool UTM_zone_northp_{true};
+
+  std::vector<rclcpp::Time> last_gps_stamp_;
+
+  rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr navsat_pub_{nullptr};
+  std::string navsatfix_topic_;
 
-  std::vector<double> last_gps_stamp_;
+  bool first_pose_received_{false};
 
 };
 
diff --git a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp
index d9dbde9c..1c7f4d35 100644
--- a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp
+++ b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp
@@ -64,7 +64,6 @@
 #include "tf2/LinearMath/Transform.hpp"
 #include <tf2_ros/buffer.hpp>
 #include <tf2_ros/transform_broadcaster.hpp>
-#include <tf2_ros/transform_listener.hpp>
 
 namespace robot_localization
 {
@@ -108,7 +107,8 @@ class UkfWrapper
   explicit UkfWrapper(
     std::shared_ptr<easynav::LocalizerNode> parent_node,
     const std::string & tf_prefix,
-    const std::string & plugin_name);
+    const std::string & plugin_name,
+    bool local_filter);
 
   //! @brief Destructor
   //!
@@ -344,7 +344,7 @@ class UkfWrapper
   //!
   bool validateFilterOutput(nav_msgs::msg::Odometry * message);
 
-  std::vector<CallbackData> getGpsCallbackDataArr() const
+  const std::vector<CallbackData> & getGpsCallbackDataArr() const
   {
     return gps_callbackData_arr_;
   }
@@ -820,13 +820,9 @@ class UkfWrapper
   //!
   rclcpp::Service<std_srvs::srv::Empty>::SharedPtr reset_srv_;
 
-  //! @brief Transform buffer for managing coordinate transforms
+  //! @brief Transform buffer for managing coordinate transforms (shared RTTFBuffer singleton)
   //!
-  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
-
-  //! @brief Transform listener for receiving transforms
-  //!
-  std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
+  tf2_ros::Buffer * tf_buffer_{nullptr};
 
   //! @brief broadcaster of worldTransform tfs
   //!
@@ -870,6 +866,7 @@ class UkfWrapper
   std::string plugin_name_;
   std::string tf_prefix_;
   std::vector<CallbackData> gps_callbackData_arr_;
+  bool local_filter_;
 
 };
 
diff --git a/localizers/easynav_fusion_localizer/maps/empty.png b/localizers/easynav_fusion_localizer/maps/empty.png
new file mode 100644
index 0000000000000000000000000000000000000000..cd6eb69a9179a64f6596a543a2ed9dbc40bb35fc
GIT binary patch
literal 4849
zcmeAS@N?(olHy`uVBq!ia0y~yV15C@985rwLkFEVGcYi=WI8(scse^P6cpvBW#*(Z
zFlbDyow(oou!GFe_+ZzGT}`rG7AY(I$^s|$ZRK)ZA&}J?ChMd3m(^Ell8Ctb!L1J-
z^dDW-+`V}n-?}D+AIwMW9p;F{E;;gEwN=bePUic=yL*q{wRXq|o0;}ig~50GSsyLU
zXP;Il-PdMuG3c>inHhO(j>^)9*8AQGK6oA5lm2u5?_!=+tlf-^6D<z%TvofMFz@ab
z-gCc?B!*23eN_C&bF!YtpA)4=ADf?3oz6N>(rok0%0~tpBVHvf?T);8r6av#@v3FP
z`zCyjc)I6k<mzjYzvumFUD&j5)0xs)$sUP<-V+p@9A!mA5|!On`*pSQyj8FLvv^&+
z(sZYFToRUI0nKbTzc&9r_&#s#qc2mYi+C~Z)$jXk$oJ(f)7rMjHS*SeuUR<kn8T}%
z$7eHr^uHu6t6RmwlgliAUDo#|vtD__iOGtyH)*qGavfFrQ0I1k3cFigW25Q%?cePD
z<QsHKr|x6q<7?~`N?reZ&+h%Zx7VNF^7}XAoV!KabMj{cV}*gUz$3Dlfk96hgc&QA
z+Lr+Z*-JcqUD;nTNeEa-i|)!W1`0`*xJHyX=jZ08=9Mrw7o{eaq^2m8XO?6rxO@5r
zgg5euGcX9;_H=O!skrs_njs?t1IJ;51N)yRHcnQacwzP2B@7G=_uYUA!QdT`Rmjf3
z@R*N*L86?2;lK`Nh(M1u14F_cMz8?GF<Ax%m;hKQNC3ZHm|?i}f=q|o3pWg84OYD%
zmw^ofDaEdruqROTQo|D{fkzcjAO`^0bXdS+4*<fRATbsRdjcf|V2=qBJpqbvxV?m9
z5f&mS4nc_tlw^!GIict!><N@)j6Ehu^u#bsim{_53ZZmNI4z>2V|b=O&QPSLMP$9B
zB?_pvgB2j~j0_V%DHTv+VzfkoH5HJP6RnftXo*6Fh89XX9xYL@rDGzBMG~v`(GmsP
zfxum)4oFfQEm1~G6fBK2!f6pD9TV;_jFu=^8(N@nh6f(uzAM(|a>H!ah6Jv&ReOqb
PL4ziqu6{1-oD!M<xv|h9

literal 0
HcmV?d00001

diff --git a/localizers/easynav_fusion_localizer/maps/empty.yaml b/localizers/easynav_fusion_localizer/maps/empty.yaml
new file mode 100644
index 00000000..629ca4cb
--- /dev/null
+++ b/localizers/easynav_fusion_localizer/maps/empty.yaml
@@ -0,0 +1,7 @@
+image: empty.png
+mode: trinary
+resolution: 0.2
+origin: [-100.0, -100.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
diff --git a/localizers/easynav_fusion_localizer/maps/square.png b/localizers/easynav_fusion_localizer/maps/square.png
new file mode 100644
index 0000000000000000000000000000000000000000..2e6d85275d3d37d8ebd8bca680f043604c99d85c
GIT binary patch
literal 11002
zcmeHMc~lc=5^s({MFm+eK-4yjD~u-T9ORNi1>`cqa44*@+DSSXO-M)r2{>TH=RKZ-
zI)W&x!YB&rIEn`fjv_h^qRZ~W0BXEPxuT2;D!5;F5``Uo`y6%u+3+5rySl2rs$c!;
ztFOb#i0MHC?Op952pSj?91saXHoxM(eeHmRzv~}&z#l_ebgViO)xZj+Tr81dusT(N
z!I)Meh9GVInlERqwH?#<<+C9wTiZbH+OBU!@`IH(B9qe6HijJi(>xD}2W?xKU$x;^
z{ZC`=M*cIpU-0IyM`VT$ZZK^4yTK-5Y|_*7%WJKcJiPkzg}R(vy;D$i)$(&OWv)dq
zE1;7%ud1`<vi$n`rUdtyaMJXGvx^K<e=PQ>`brZx|43tTO0zoX)PxYU$~7!Ad|LjV
zF~!sN&fO6-{CVRqQ+Ig}e@>clI@Be*Bg?~<x-%+Y9VNNlFqfN$gig+k)NVMg{V{5a
zbykk!<9$!D({1<auMU5C|1UQkonkfPm~k1WAjoQ|#NR(6#NXeXJIHuj`i8Qwvy<E^
z=0xR3j<-*VNV*V_+q{#s_K1Dpo<Z}M&KNn3()AhNb>ooXL1&KW_3J7Mm#jWq>`<Hu
zci5cso3w36{O)sYW0wv)G|KJS&1<JBuT7k^XUkS`as|XMsfs31#NY1|=!aS_qv_x@
zOzfiCm=#d@DA{rJoVK0b``VBD1?|W_?5rQWQus3>yEX9ER(^xNG~7)b7&q2;+;-$a
z=DBsVXL@Jco~?+}Z$%lo+2b4ruU^WUQUC2w{u*sIXXkw0-Tp4?VlL0xc0{?!tL@?(
z=bs*ozcl+h?kd}1hiv<ihD3ik#f7!=hljgkgRf?kE?IQ$N!Xpp3o+L2mDXQeflp=_
z{5q}`ZQ7Cj+r}wLc`quGe9!C3DOdJzFF!kUn>ihN#Zcszyxhb?UWB9-<p-|Sb<6a(
zPUL$oEWhi3{3g0k&>WhPJ;m3tbjMXTw)o5}_}INC|7xs!V6(oUcq6;~%oyaadCgB8
z+FB)TEjJid-5Jo7cI7vzS6602kfW~zkTI4&jVF}LD5yv-z$jXo0<aW<y#2HaRJZ_B
z!vZWplITNfJ6T48B_bcvOfNpdSNLP|B*AG)EGlh!v@mUfkSikj`PzGHdBA}TQ=_m}
zCQVfFv_2%lFArSfVk!wHRMZQ6NU{6~*k7*1U?zo0LCAqxNeZ3hYY%%XMPgoLz*Lh9
zaOXpsr&cR?RH{a!p=cNsxiW!D<8rxFgifW?$v}gwN=;OwT5_Vw9d}{$5rC<LN{K=(
zktf2qPgEdJR{M}h;5lrLPp07WU%O9KnNR?DP_?LnN~0iDnT*=gLZuE&0ZvQ-z12b$
z4SJdyiK*ntN+A}Qf+ecmd%6$_U)w8^l~N)dk&uc>F&R)*fmLa}mJABvN4&PcAxMzO
z6oeH3yO*X~B7Or_uh{S>L^?eK0p_p$_R==zPACH{KA#sL7bfH3g#`GJ@b!5jxlkhF
z5tkeRmrLWaxMVidOF(8K7>z8TalFVZwur$6)IeAqMh{mZi7GXkD8z7Az&S+%{9ppM
z2oo~dWHt+YFcATrEMU_ZWR6(CMQL;qSIG40;UZiq0jxx&J)^>1iGVAH0Oc^nVg{MU
zpb5!L4qym}<HaH)47!L#<FLhaA(wC^67r_Xl`<5RQzAnXFsdRkfp~xe&YK(&;zOcS
zkT;SDDXJC&10T{fNn*10&4Xx(42x2uIGQvzn~pHNI1GeCqcIU<+JwIum{J9pi2I}=
z6b6eh$IHS4$pBzcoTtD6AqTne{FNB0mMf#>a;XmqcLC#;uOIV4JBd&=8i1-X;1r>=
zcnF<G_ll;mc{GH_K*l399@0Z!E|Q2-|C=`6Jg~R1<iQdZSU;5z89OQpOETUWZ>18V
zm0*}?3LYvnrl3MoFcA?a@MU}?oQEbRU|{z!(bX)MyoCxhp@7N8ONW9|k(nYkMiu}O
zS;Xc7sesEwX=rcnD!Ev#L6z9#1b`#J3ec0l3icoh<=I<XGY`Y7001K+bTSSYi^ufh
zAzTw+)K8xxhO&h$E{8#;Gesy!Q6M67Xmpe;#5fpD#1zmF)X4VN0DfqSY(NDbjrqSz
z5oNOIUSbR(bC?*YqL?cta}ZQSrg4O9j42ihx$KXgVo%zes_0F{cYZT(y{W%rwo#$S
zr~uVB%D@N$6OH<Mq?u>|O7baxOda(pmw;i*BJYK-1uhF*?}fm7DO=cOf$O~xcrRrO
zyFMCR_HSN_u|#l|)qq#tddD*q@Vaa#2nz}zUYwy$|C5WrRX;^=oC<;{?)a~jHs3D=
z2>Yl*_<?;MTDuIgABDIb*aJi^>cCjFzg&jDTS3H;9}~eEiFzK4ANgl4b{Gyp&Z|QL
zCP!=QTdvVoHP*Q<yZLJN0K1@~;6tpu@+L#3{Wf;}x{;|a_T!y4g={EvU+j71b5+$+
zmpS1JqtkasRS(WCc+kH0Z(4Fp@~RbHU`^frZlN{!I|zI|1?eXB0iTN<z(-%TMff3r
zOv<w{o7?itcv^%6!TvBHko8CaC1DO<v4S81PmAya0U=tSz|+|J1jrU4PO$d|<eDuo
zvS3M!kdMYA%UJppBl!>S!p3q~%<&V_e@^uO)3LPb*=1Yk<$>gQP5a?#HAB*GRC-tY
zb!DT@x757j3aI>M{)*TtsC<-7dM!22)5Qv^%L;0I`vC7)UEK6Fv4>tLlDL8+^Se)?
z`rLj{$AQ8^R#%2+ep%PE;->CL<Ic~F87ga-8edBp1V#KX#A%-kv|@#GWm3(c?zp_R
zitoaA+`6=d8n}HR#2K~jn2q7=u50<-&z~&Vopr1Iel^7xTAw@dSLo%z;)c_oyFH+^
zFQ{&H;6VqyG^PVbL}5|e`L>HpJ_W%ydbs3t+ZrOs3$pDAAYAK_I-y6WlLnt1<!ECf
z+n9oK*5-=7_*3HnW{Aw$4m`*SbGjMz6$nt~SJ!g)?%3_yx3BVE7<!S~A3Df2uLvy6
zW&|$!@c3E)A(+rvlDN%hU+(AE9Gi=u-Sei^HvN-WMAt~!vf9aXR005UvI+|UsNm{C
zk!+SRO8T1H+HZFpDPAyq0O9MwSH2^xApOwDfc;_!CJEw87KQ|S4qe#lo(r&Ycvm#X
za>#yC2fPss5hscn)_5RrNrA*zL!v{<3qms-pPLcmENf_)(frHv)0)LrI7YMymm-^g
zxtE$k+!D#^cE-eDV3IC&n3O1MaJ9m{ub&lPJC%4(Q4BbXI9ox-Rujdodo15lR$6+l
zrY34bLbzuqW(BqWIMVd?lC|ef*%Al5PMoisp3FD7+;XXBDU6sV6R`tDj~dR}<(+Xk
z1xaU!0JdJff7poRaUVZFfRMT6b^dz`f9i{;t81)3>SYZb?C(jM!GHjBPPy&QTIKU>
z!(}_jZ8cz1GwV02w;Wq_!uC-hP=JBMyLe&@?sS2KZv76n8Tmx$9XrsYb+W5Y*M2{t
zqLn#d$(&($(24=IO-Jeu=T09t+!-&p+u6HH8c{Cb!aQJ}&UFhi5;h|!CMGBSKzpY&
z0@B@BPZD_9lnX~x(jK0;esAg6eE;0^_*y}=9nL9-dtMv90PWFw*1Z+--cJNUP>tx{
zUdI5Bn$4qGecqgq<oShP80q-ft;OltQCU0_n#+z1(4K{L2e03G@ztwtpX+6d<}@FV
z3eAXvbSIPgjMP<4xFjG-Y3_`79aasj-vhT6joU1!W>p{VKLC$??aj1PCW1Be#v^dV
zvNJ?^_0H#$COnmXS99?*JG_dO3f~=|<qbJRr)Z^3C#u3GhwiOYjc$=b<&Ta5!kId@
zS6q_<0@U#dCJORvq>s~&KL1?%26}DSsFpss|3F^7d+RKpdPf|lx=jr&Vw2IO?*Z-c
zMmt$p2>3uW+EEz@1?XKUDsEctT`~^5z=K*&)L$et%N@KQeq)1cnrQ-3CVJ8^Y%^x8
zK3vkXGxF;W^K0%{+u&P{vgyTj6T$DLvIqilmL<8F7HlE*3%oNM+S^Sn(pZfQ08QFT
z2|PF7PFH!PxS@0a(F#}bR`>?A0ue0uG@TH2k<ur=;Oh`G|A+=RP*cy1#1lPeQ_@gn
z+R)75nr%TM+EWD8nlDcr3>oC<9fmQd(_aj0_kPt;LQI~P$M?HB9_?Cf2Q^P$)pyZ|
zKKjKio#DIr_<)j+>b}hZQ{vUj3i{YG4;&(Wl1)0Bx)~&{wYk2;!BAd0e1tIyevJ`$
zuy(JV*fjrvP&c|)NeumKn8q!-aVFTdI+hYcqI9L^&yaSuXa1ZFer@tqX4R{%?83sG
zvIf$UMcWgMLBM4-52AmL8`b4E0@9_2IUUNzUqc>0#y1Mfmo!Eq_ack=Tb4f}1Qauy
zTaL4)jp2g<c~^@7{t_UA&FW3xScIkxk)Y!5q00dPSz!Cfl<75=0lvLXDB#{So4}Pt
h6K~eH0;1d24M~%iyK4Hy;{W4-LIS4;6i=C(`EM3mI}HE;

literal 0
HcmV?d00001

diff --git a/localizers/easynav_fusion_localizer/maps/square.yaml b/localizers/easynav_fusion_localizer/maps/square.yaml
new file mode 100644
index 00000000..34ce4863
--- /dev/null
+++ b/localizers/easynav_fusion_localizer/maps/square.yaml
@@ -0,0 +1,7 @@
+image: square.png
+mode: trinary
+resolution: 0.2
+origin: [-100.0, -100.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
diff --git a/localizers/easynav_fusion_localizer/package.xml b/localizers/easynav_fusion_localizer/package.xml
index a245a547..6d2175c6 100644
--- a/localizers/easynav_fusion_localizer/package.xml
+++ b/localizers/easynav_fusion_localizer/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>easynav_fusion_localizer</name>
-  <version>0.2.1</version>
+  <version>0.0.2</version>
   <description>Easy Navigation: Fusion Localizer package.</description>
   <maintainer email="miguelangel.demgiuel@urjc.es">Miguel Ángel de Miguel</maintainer>
   
diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp
index b212b73f..7982a9a7 100644
--- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp
+++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp
@@ -6,6 +6,7 @@
 
 #include "easynav_common/types/IMUPerception.hpp"
 #include "easynav_common/types/GNSSPerception.hpp"
+#include "sensor_msgs/msg/nav_sat_status.hpp"
 
 #include <GeographicLib/UTMUPS.hpp>
 
@@ -23,8 +24,6 @@ void FusionLocalizer::on_initialize()
 
     auto localizer_node = std::dynamic_pointer_cast<LocalizerNode>(node);
 
-    last_gps_stamp_.resize(10, 0.0);
-
     const std::string & plugin_name = this->get_plugin_name();
     const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
 
@@ -32,75 +31,157 @@ void FusionLocalizer::on_initialize()
     RCLCPP_INFO(localizer_node->get_logger(), "Using parameter namespace: '%s'",
     plugin_name.c_str());
 
-    ukf_wrapper_ = std::make_unique<robot_localization::UkfWrapper>(
-      localizer_node, tf_info.tf_prefix, plugin_name + ".local_filter"
-    );
-    ukf_wrapper_->initialize();
-    localizer_node->declare_parameter(plugin_name + ".latitude_origin", double(0.0));
-    localizer_node->get_parameter(plugin_name + ".latitude_origin", latitude_origin_);
+    // Detect which filters have parameters configured by checking
+    // parameter overrides (loaded from YAML before declaration)
+    const std::string global_prefix = plugin_name + ".global_filter.";
+    const std::string local_prefix = plugin_name + ".local_filter.";
+
+    const auto & overrides =
+      localizer_node->get_node_parameters_interface()->get_parameter_overrides();
+
+    for (const auto & [key, _] : overrides) {
+      if (!has_global_filter_ && key.rfind(global_prefix, 0) == 0) {
+        has_global_filter_ = true;
+      }
+      if (!has_local_filter_ && key.rfind(local_prefix, 0) == 0) {
+        has_local_filter_ = true;
+      }
+      if (has_global_filter_ && has_local_filter_) {
+        break;
+      }
+    }
+
+    if (!has_global_filter_ && !has_local_filter_) {
+      RCLCPP_FATAL(
+        localizer_node->get_logger(),
+        "No parameters found for either '%s' or '%s'. "
+        "At least one filter must be configured.",
+        global_prefix.c_str(), local_prefix.c_str());
+      throw std::runtime_error(
+        "FusionLocalizer: no global_filter or local_filter parameters detected. "
+        "At least one filter must be configured.");
+    }
+
+    if (has_global_filter_) {
+      ukf_global_ = std::make_unique<robot_localization::UkfWrapper>(
+        localizer_node, tf_info.tf_prefix, plugin_name + ".global_filter", false
+      );
+      ukf_global_->initialize();
+    } else {
+      RCLCPP_WARN(localizer_node->get_logger(),
+          "No global_filter parameters found. Global filter will NOT be created.");
+    }
+
+    if (has_local_filter_) {
+      ukf_local_ = std::make_unique<robot_localization::UkfWrapper>(
+        localizer_node, tf_info.tf_prefix, plugin_name + ".local_filter", true
+      );
+      ukf_local_->initialize();
+    } else {
+      RCLCPP_WARN(localizer_node->get_logger(),
+          "No local_filter parameters found. Local filter will NOT be created.");
+    }
+
+    // GPS-related setup only needed when global filter is active
+    if (has_global_filter_) {
+      localizer_node->declare_parameter(plugin_name + ".latitude_origin", double(0.0));
+      localizer_node->get_parameter(plugin_name + ".latitude_origin", latitude_origin_);
+
+      localizer_node->declare_parameter(plugin_name + ".longitude_origin", double(0.0));
+      localizer_node->get_parameter(plugin_name + ".longitude_origin", longitude_origin_);
 
-    localizer_node->declare_parameter(plugin_name + ".longitude_origin", double(0.0));
-    localizer_node->get_parameter(plugin_name + ".longitude_origin", longitude_origin_);
+      localizer_node->declare_parameter(plugin_name + ".altitude_origin", double(0.0));
+      localizer_node->get_parameter(plugin_name + ".altitude_origin", altitude_origin_);
+
+      localizer_node->declare_parameter(
+        plugin_name + ".navsatfix_topic", std::string("gps/filtered"));
+      localizer_node->get_parameter(plugin_name + ".navsatfix_topic", navsatfix_topic_);
+      navsat_pub_ = localizer_node->create_publisher<sensor_msgs::msg::NavSatFix>(
+        navsatfix_topic_, rclcpp::QoS(10));
+    }
 
-    localizer_node->declare_parameter(plugin_name + ".altitude_origin", double(0.0));
-    localizer_node->get_parameter(plugin_name + ".altitude_origin", altitude_origin_);
   } catch (const std::exception & e) {
     RCLCPP_FATAL(
       get_node()->get_logger(), "Critical failure initializing UkfWrapper: %s",
       e.what());
-    // Raise error
     throw std::runtime_error(std::string("Failed to initialize UkfWrapper: ") + e.what());
   }
 
+  if (has_global_filter_) {
+    GeographicLib::UTMUPS::Forward(latitude_origin_, longitude_origin_, UTM_zone_number_,
+        UTM_zone_northp_, UTM_origin_x_, UTM_origin_y_);
+    UTM_zone_ = std::to_string(UTM_zone_number_) + (UTM_zone_northp_ ? "N" : "S");
+    UTM_origin_z_ = altitude_origin_;
 
-  int zone;
-  bool northp;
-
-  GeographicLib::UTMUPS::Forward(latitude_origin_, longitude_origin_, zone, northp, UTM_origin_x_,
-      UTM_origin_y_);
-  UTM_zone_ = std::to_string(zone) + (northp ? "N" : "S");
-  UTM_origin_z_ = altitude_origin_;
-
-  n_gps_sensors_ = static_cast<int>(ukf_wrapper_->getGpsCallbackDataArr().size());
+    n_gps_sensors_ = static_cast<int>(ukf_global_->getGpsCallbackDataArr().size());
+    last_gps_stamp_.resize(n_gps_sensors_, rclcpp::Time(0, 0, RCL_ROS_TIME));
+  }
 
   RCLCPP_INFO(get_node()->get_logger(), "FusionLocalizer (UKF) initialized successfully.");
 }
 
-// 2. Hook de actualización RT (Tu "Timer" de alta frecuencia)
 void FusionLocalizer::update_rt(NavState & nav_state)
 {
-  if(n_gps_sensors_ && nav_state.has("gnss")) {
-    auto gps_data = nav_state.get<GNSSPerceptions>(std::string("gnss"));
-    for (int i = 0; i < n_gps_sensors_; ++i) {
-      double gps_time = gps_data[i]->data.header.stamp.sec +
-        gps_data[i]->data.header.stamp.nanosec * 1e-9;
-      if (gps_time > last_gps_stamp_[i]) {
-      // if(true) {
-        last_gps_stamp_[i] = gps_time;
-        auto pose = navsatfix_to_pose(gps_data[i]->data);
-        // nav_state.set("UTM_gnss_pose", pose);
-        // Call the wrapper callback
-        const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
-        ukf_wrapper_->poseCallback(
-          std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>(pose),
-          ukf_wrapper_->getGpsCallbackDataArr()[i], // callback_data
-          tf_info.map_frame, // target_frame
-          tf_info.odom_frame,  // pose_source_frame
-          false                           // imu_data
-        );
+  const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
+
+  if (has_global_filter_) {
+    if (n_gps_sensors_ && nav_state.has("gnss")) {
+      auto gps_data = nav_state.get<GNSSPerceptions>(std::string("gnss"));
+      const auto & gps_cb_arr = ukf_global_->getGpsCallbackDataArr();
+      for (int i = 0; i < n_gps_sensors_; ++i) {
+        if (gps_data[i]->data.status.status < sensor_msgs::msg::NavSatStatus::STATUS_FIX) {
+          continue;
+        }
+        rclcpp::Time gps_time(gps_data[i]->data.header.stamp);
+        if (gps_time > last_gps_stamp_[i]) {
+          EASYNAV_TRACE_NAMED_EVENT("fusion_localizer_process_gps");
+          last_gps_stamp_[i] = gps_time;
+          auto pose =
+            std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>(navsatfix_to_pose(
+              gps_data[i]->data));
+          if (!first_pose_received_) {
+            RCLCPP_INFO(get_node()->get_logger(),
+                "First valid GPS fix received. Initializing filter state.");
+            if(nav_state.has("imu")) {
+              auto imu_data = nav_state.get<IMUPerceptions>(std::string("imu"));
+              if (!imu_data.empty()) {
+                pose->pose.pose.orientation = imu_data[0]->data.orientation;
+              }
+            }
+            ukf_global_->setPoseCallback(pose);
+            first_pose_received_ = true;
+            continue;
+          }
+          ukf_global_->poseCallback(
+            pose,
+            gps_cb_arr[i],
+            tf_info.map_frame,
+            gps_data[i]->data.header.frame_id,
+            false
+          );
+        }
       }
     }
+
+    ukf_global_->periodicUpdate();
+
+    nav_msgs::msg::Odometry global_odom;
+    if (ukf_global_->getFilteredOdometryMessage(&global_odom)) {
+      nav_state.set("robot_pose", global_odom);
+      navsat_pub_->publish(odom_to_navsatfix(global_odom));
+    }
   }
 
-  ukf_wrapper_->periodicUpdate();
+  if (has_local_filter_) {
+    ukf_local_->periodicUpdate();
 
-  nav_msgs::msg::Odometry current_odom;
-  if (ukf_wrapper_->getFilteredOdometryMessage(&current_odom)) {
-    nav_state.set("robot_pose", current_odom);
+    nav_msgs::msg::Odometry local_odom;
+    if (ukf_local_->getFilteredOdometryMessage(&local_odom)) {
+      nav_state.set("robot_pose_local", local_odom);
+    }
   }
 }
 
-// 3. Hook de actualización no-RT (baja frecuencia)
 void FusionLocalizer::update([[maybe_unused]] NavState & nav_state)
 {
 
@@ -110,16 +191,12 @@ geometry_msgs::msg::PoseWithCovarianceStamped FusionLocalizer::navsatfix_to_pose
   const sensor_msgs::msg::NavSatFix & navsat_msg)
 {
   geometry_msgs::msg::PoseWithCovarianceStamped pose_msg;
-  const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
 
-  // 1. Establecer el Header
-  // Usamos el mismo timestamp que el mensaje original
-  // y el world_frame_id que el filtro UKF espera (p.ej., "map" u "odom")
   pose_msg.header = navsat_msg.header;
 
+  const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
   pose_msg.header.frame_id = tf_info.map_frame;
 
-  // 2. Convertir coordenadas (Lat, Lon) a UTM (x, y)
   double utm_x, utm_y;
   int zone;
   bool northp;
@@ -130,7 +207,8 @@ geometry_msgs::msg::PoseWithCovarianceStamped FusionLocalizer::navsatfix_to_pose
     zone,
     northp,
     utm_x,
-    utm_y);
+    utm_y,
+    UTM_zone_number_);
 
   pose_msg.pose.pose.position.x = utm_x - UTM_origin_x_;
   pose_msg.pose.pose.position.y = utm_y - UTM_origin_y_;
@@ -143,23 +221,65 @@ geometry_msgs::msg::PoseWithCovarianceStamped FusionLocalizer::navsatfix_to_pose
 
   pose_msg.pose.covariance.fill(0.0);
 
-  pose_msg.pose.covariance[0] = navsat_msg.position_covariance[0];  // xx
-  pose_msg.pose.covariance[1] = navsat_msg.position_covariance[1];  // xy
-  pose_msg.pose.covariance[2] = navsat_msg.position_covariance[2];  // xz
+  double default_var = 1.0; // 1 meter variance standard
+
+  if (navsat_msg.position_covariance_type ==
+    sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN)
+  {
+    pose_msg.pose.covariance[0] = navsat_msg.position_covariance[0];
+    pose_msg.pose.covariance[7] = navsat_msg.position_covariance[4];
+    pose_msg.pose.covariance[14] = navsat_msg.position_covariance[8];
+
+    pose_msg.pose.covariance[21] = default_var;
+    pose_msg.pose.covariance[28] = default_var;
+    pose_msg.pose.covariance[35] = default_var;
+  } else {
+      // Fallback variances if GPS doesn't provide them
+    pose_msg.pose.covariance[0] = default_var;
+    pose_msg.pose.covariance[7] = default_var;
+    pose_msg.pose.covariance[14] = default_var;
+
+    pose_msg.pose.covariance[21] = default_var;
+    pose_msg.pose.covariance[28] = default_var;
+    pose_msg.pose.covariance[35] = default_var;
+
+    RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 5000,
+        "NavSatFix covariance type unknown or invalid. Using default covariance.");
+  }
 
-  pose_msg.pose.covariance[6] = navsat_msg.position_covariance[3];  // yx
-  pose_msg.pose.covariance[7] = navsat_msg.position_covariance[4];  // yy
-  pose_msg.pose.covariance[8] = navsat_msg.position_covariance[5];  // yz
+  return pose_msg;
+}
 
-  pose_msg.pose.covariance[12] = navsat_msg.position_covariance[6]; // zx
-  pose_msg.pose.covariance[13] = navsat_msg.position_covariance[7]; // zy
-  pose_msg.pose.covariance[14] = navsat_msg.position_covariance[8]; // zz
+sensor_msgs::msg::NavSatFix FusionLocalizer::odom_to_navsatfix(
+  const nav_msgs::msg::Odometry & odom_msg)
+{
+  sensor_msgs::msg::NavSatFix navsat_msg;
 
-  pose_msg.pose.covariance[21] = 99999.0;
-  pose_msg.pose.covariance[28] = 99999.0;
-  pose_msg.pose.covariance[35] = 99999.0;
+  navsat_msg.header = odom_msg.header;
+  navsat_msg.header.frame_id = "gps_link";
+  navsat_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX;
+  navsat_msg.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GPS;
 
-  return pose_msg;
+  const double utm_x = odom_msg.pose.pose.position.x + UTM_origin_x_;
+  const double utm_y = odom_msg.pose.pose.position.y + UTM_origin_y_;
+
+  double latitude = 0.0;
+  double longitude = 0.0;
+  GeographicLib::UTMUPS::Reverse(
+    UTM_zone_number_, UTM_zone_northp_, utm_x, utm_y, latitude, longitude);
+
+  navsat_msg.latitude = latitude;
+  navsat_msg.longitude = longitude;
+  navsat_msg.altitude = odom_msg.pose.pose.position.z + UTM_origin_z_;
+
+  navsat_msg.position_covariance[0] = odom_msg.pose.covariance[0];
+  navsat_msg.position_covariance[4] = odom_msg.pose.covariance[7];
+  navsat_msg.position_covariance[8] = odom_msg.pose.covariance[14];
+
+  navsat_msg.position_covariance_type =
+    sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;
+
+  return navsat_msg;
 }
 
 }  // namespace easynav
diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp
index ec2f87da..ebd38f08 100644
--- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp
+++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp
@@ -75,7 +75,6 @@
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include <tf2_ros/buffer.hpp>
 #include <tf2_ros/transform_broadcaster.hpp>
-#include <tf2_ros/transform_listener.hpp>
 
 #include "easynav_common/RTTFBuffer.hpp"
 
@@ -86,7 +85,8 @@ using namespace std::chrono_literals;
 UkfWrapper::UkfWrapper(
   std::shared_ptr<easynav::LocalizerNode> parent_node,
   const std::string & tf_prefix,
-  const std::string & plugin_name)
+  const std::string & plugin_name,
+  bool local_filter)
 : print_diagnostics_(true),
   publish_acceleration_(false),
   publish_transform_(true),
@@ -114,14 +114,15 @@ UkfWrapper::UkfWrapper(
   last_set_pose_time_(0, 0, RCL_ROS_TIME),
   latest_control_time_(0, 0, RCL_ROS_TIME),
   tf_timeout_(0ns),
-  tf_time_offset_(0ns)
+  tf_time_offset_(0ns),
+  local_filter_(local_filter)
 {
   parent_node_ = parent_node;
   tf_prefix_ = tf_prefix;
   plugin_name_ = plugin_name;
 
-  tf_buffer_ = std::make_unique<tf2_ros::Buffer>(parent_node_->get_clock());
-  tf_listener_ = std::make_unique<tf2_ros::TransformListener>(*tf_buffer_);
+  // Use the shared RTTFBuffer singleton instead of creating a private tf2_ros::Buffer
+  tf_buffer_ = easynav::RTTFBuffer::getInstance();
 
   state_variable_names_.push_back("X");
   state_variable_names_.push_back("Y");
@@ -147,8 +148,6 @@ UkfWrapper::~UkfWrapper()
   set_pose_sub_.reset();
   control_sub_.reset();
   stamped_control_sub_.reset();
-  tf_listener_.reset();
-  tf_buffer_.reset();
   diagnostic_updater_.reset();
   world_transform_broadcaster_.reset();
   set_pose_service_.reset();
@@ -184,8 +183,7 @@ void UkfWrapper::reset()
 
   last_diff_time_ = parent_node_->now().seconds();
 
-  // clear tf buffer to avoid TF_OLD_DATA errors
-  tf_buffer_->clear();
+  // Note: tf_buffer_ is the shared RTTFBuffer singleton, do not clear it here
 
   // clear last message timestamp, so older messages will be accepted
   last_message_times_.clear();
@@ -370,7 +368,7 @@ void UkfWrapper::enqueueMeasurement(
   const std::vector<bool> & update_vector, const double mahalanobis_thresh,
   const rclcpp::Time & time)
 {
-  MeasurementPtr meas = MeasurementPtr(new Measurement());
+  MeasurementPtr meas = std::make_shared<Measurement>();
 
   meas->topic_name_ = topic_name;
   meas->measurement_ = measurement;
@@ -895,7 +893,11 @@ void UkfWrapper::loadParams()
   odom_frame_id_ = tf_info.odom_frame;
   base_link_frame_id_ = tf_info.robot_frame;
   // World frame comes from Easynav TFInfo configuration
-  world_frame_id_ = tf_info.map_frame;
+  if(local_filter_) {
+    world_frame_id_ = tf_info.odom_frame;
+  } else {
+    world_frame_id_ = tf_info.map_frame;
+  }
 
   base_link_output_frame_id_ = base_link_frame_id_;
 
@@ -1172,30 +1174,39 @@ void UkfWrapper::loadParams()
       "\npermit_corrected_publication is " << permit_corrected_publication_ <<
       "\nprint_diagnostics is " << print_diagnostics_ << "\n");
 
+  // Determine mode prefix for service/topic names to avoid collisions
+  // between local and global filter instances
+  std::string mode_prefix;
+  if (local_filter_) {
+    mode_prefix = "local_";
+  } else {
+    mode_prefix = "global_";
+  }
+
   // Create a subscriber for manually setting/resetting pose
   set_pose_sub_ =
     parent_node_->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
-    "set_pose", rclcpp::QoS(1),
+    mode_prefix + "set_pose", rclcpp::QoS(1),
     std::bind(&UkfWrapper::setPoseCallback, this, std::placeholders::_1), options);
 
   // Create a service for manually setting/resetting pose
   set_pose_service_ =
     parent_node_->create_service<robot_localization::srv::SetPose>(
-    "set_pose", std::bind(
+    mode_prefix + "set_pose", std::bind(
       &UkfWrapper::setPoseSrvCallback, this,
       std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 
   // Create a service for manually enabling the filter
   enable_filter_srv_ =
     parent_node_->create_service<std_srvs::srv::Empty>(
-    "~/enable", std::bind(
+    "~/" + mode_prefix + "enable", std::bind(
       &UkfWrapper::enableFilterSrvCallback, this,
       std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 
   // Create a service for manually setting/resetting pose
   reset_srv_ =
     parent_node_->create_service<std_srvs::srv::Empty>(
-    "reset", std::bind(
+    mode_prefix + "reset", std::bind(
       &UkfWrapper::resetSrvCallback, this,
       std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 
@@ -1203,7 +1214,7 @@ void UkfWrapper::loadParams()
   // publishing
   toggle_filter_processing_srv_ =
     parent_node_->create_service<robot_localization::srv::ToggleFilterProcessing>(
-    "~/toggle", std::bind(
+    "~/" + mode_prefix + "toggle", std::bind(
       &UkfWrapper::toggleFilterProcessingCallback, this,
       std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 
@@ -2090,6 +2101,7 @@ void UkfWrapper::odometryCallback(
     pos_ptr->header = msg->header;
     pos_ptr->pose = msg->pose;  // Entire pose object, also copies covariance
 
+
     if (pose_callback_data.pose_use_child_frame_) {
       poseCallback(pos_ptr, pose_callback_data, world_frame_id_, msg->child_frame_id, false);
     } else {
@@ -2262,15 +2274,21 @@ void UkfWrapper::initialize()
   // Position publisher
   rclcpp::PublisherOptions publisher_options;
   publisher_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
+  std::string mode_prefix;
+  if (local_filter_) {
+    mode_prefix = "local_";
+  } else {
+    mode_prefix = "global_";
+  }
   position_pub_ =
     parent_node_->create_publisher<nav_msgs::msg::Odometry>(
-    "odometry/filtered", rclcpp::QoS(10), publisher_options);
+    mode_prefix + "odometry/filtered", rclcpp::QoS(10), publisher_options);
 
   // Optional acceleration publisher
   if (publish_acceleration_) {
     accel_pub_ =
       parent_node_->create_publisher<geometry_msgs::msg::AccelWithCovarianceStamped>(
-      "accel/filtered", rclcpp::QoS(10), publisher_options);
+      mode_prefix + "accel/filtered", rclcpp::QoS(10), publisher_options);
   }
 
   // Block commented as it is handled in the update_rt
@@ -2864,7 +2882,7 @@ bool UkfWrapper::prepareAcceleration(
   // we have to handle the situation.
   tf2::Transform target_frame_trans;
   bool can_transform = ros_filter_utilities::lookupTransformSafe(
-    tf_buffer_.get(), target_frame, msg_frame, msg->header.stamp, tf_timeout_,
+    tf_buffer_, target_frame, msg_frame, msg->header.stamp, tf_timeout_,
     target_frame_trans);
 
   if (can_transform) {
@@ -3093,7 +3111,7 @@ bool UkfWrapper::preparePose(
   // 2. Get the target frame transformation
   tf2::Transform target_frame_trans;
   bool can_transform = ros_filter_utilities::lookupTransformSafe(
-    tf_buffer_.get(), final_target_frame, pose_tmp.frame_id_,
+    tf_buffer_, final_target_frame, pose_tmp.frame_id_,
     rclcpp::Time(tf2::timeToSec(pose_tmp.stamp_)), tf_timeout_,
     target_frame_trans);
 
@@ -3103,7 +3121,7 @@ bool UkfWrapper::preparePose(
   bool can_src_transform = false;
   if (source_frame != base_link_frame_id_) {
     can_src_transform = ros_filter_utilities::lookupTransformSafe(
-      tf_buffer_.get(), source_frame, base_link_frame_id_,
+      tf_buffer_, source_frame, base_link_frame_id_,
       rclcpp::Time(tf2::timeToSec(pose_tmp.stamp_)), tf_timeout_,
       source_frame_trans);
   }
@@ -3525,7 +3543,7 @@ bool UkfWrapper::prepareTwist(
   // 4. We need to transform this into the target frame (probably base_link)
   tf2::Transform target_frame_trans;
   bool can_transform = ros_filter_utilities::lookupTransformSafe(
-    tf_buffer_.get(), target_frame, msg_frame, msg->header.stamp, tf_timeout_,
+    tf_buffer_, target_frame, msg_frame, msg->header.stamp, tf_timeout_,
     target_frame_trans);
 
   if (can_transform) {