From 2ee07e3c3db5501b680e429b23bfc07a6fed3bf3 Mon Sep 17 00:00:00 2001
From: Joel <nzmjoel@gmail.com>
Date: Wed, 25 Feb 2026 23:00:58 +0000
Subject: [PATCH 1/9] fix teleport bug

---
 utama_core/data_processing/refiners/position.py | 6 ++++++
 utama_core/run/game_gater.py                    | 1 +
 2 files changed, 7 insertions(+)

diff --git a/utama_core/data_processing/refiners/position.py b/utama_core/data_processing/refiners/position.py
index a3be811c..fea4845f 100644
--- a/utama_core/data_processing/refiners/position.py
+++ b/utama_core/data_processing/refiners/position.py
@@ -87,6 +87,9 @@ def refine(self, game_frame: GameFrame, data: List[RawVisionData]) -> GameFrame:
             for y_rbt_id, vision_y_rbt in vision_yellow.items():
                 if y_rbt_id not in self.kalman_filters_yellow:
                     self.kalman_filters_yellow[y_rbt_id] = KalmanFilter(id=y_rbt_id)
+                    if y_rbt_id not in yellow_rbt_last_frame:
+                        filtered_yellow_robots.append(vision_y_rbt)
+                        continue
 
                 filtered_robot = self.kalman_filters_yellow[y_rbt_id].filter_data(
                     vision_y_rbt,  # new measurement
@@ -100,6 +103,9 @@ def refine(self, game_frame: GameFrame, data: List[RawVisionData]) -> GameFrame:
             for b_rbt_id, vision_b_rbt in vision_blue.items():
                 if b_rbt_id not in self.kalman_filters_blue:
                     self.kalman_filters_blue[b_rbt_id] = KalmanFilter(id=b_rbt_id)
+                    if b_rbt_id not in blue_rbt_last_frame:
+                        filtered_blue_robots.append(vision_b_rbt)
+                        continue
 
                 filtered_robot = self.kalman_filters_blue[b_rbt_id].filter_data(
                     vision_b_rbt,  # new measurement
diff --git a/utama_core/run/game_gater.py b/utama_core/run/game_gater.py
index 36eb9ea3..e323dd5f 100644
--- a/utama_core/run/game_gater.py
+++ b/utama_core/run/game_gater.py
@@ -31,6 +31,7 @@ def wait_until_game_valid(
         def _add_frame(my_game_frame: GameFrame, opp_game_frame: GameFrame) -> Tuple[GameFrame, Optional[GameFrame]]:
             if rsim_env:
                 vision_frames = [rsim_env._frame_to_observations()[0]]
+                rsim_env.steps += 1  # Increment the step count to simulate time passing in the environment
             else:
                 vision_frames = [buffer.popleft() if buffer else None for buffer in vision_buffers]
             my_game_frame = position_refiner.refine(my_game_frame, vision_frames)

From f284551cb9b8818e2082c62295df9185534d6a0a Mon Sep 17 00:00:00 2001
From: Joel <nzmjoel@gmail.com>
Date: Wed, 25 Feb 2026 23:23:56 +0000
Subject: [PATCH 2/9] add teleport position test

---
 .../teleport_position_accuracy_test.py        | 167 ++++++++++++++++++
 1 file changed, 167 insertions(+)
 create mode 100644 utama_core/tests/strategy_runner/teleport_position_accuracy_test.py

diff --git a/utama_core/tests/strategy_runner/teleport_position_accuracy_test.py b/utama_core/tests/strategy_runner/teleport_position_accuracy_test.py
new file mode 100644
index 00000000..0a6f5dea
--- /dev/null
+++ b/utama_core/tests/strategy_runner/teleport_position_accuracy_test.py
@@ -0,0 +1,167 @@
+"""Tests that the first game frame after reset_field reflects teleported positions."""
+
+import os
+from typing import Optional
+
+import py_trees
+import pytest
+
+from utama_core.entities.game import Game
+from utama_core.entities.game.field import FieldBounds
+from utama_core.run import StrategyRunner
+from utama_core.strategy.common.abstract_strategy import AbstractStrategy
+from utama_core.team_controller.src.controllers import AbstractSimController
+from utama_core.tests.common.abstract_test_manager import (
+    AbstractTestManager,
+    TestingStatus,
+)
+
+os.environ["SDL_VIDEO_WINDOW_POS"] = "100,100"
+
+_TELEPORT_CASES = [
+    # (rid, x, y, theta)
+    (0, 1.0, 1.0, 0.0),
+    (0, -1.0, -1.0, 1.5707),
+    (0, 0.0, 0.0, 3.1415),
+    (1, 2.0, -1.5, 0.0),
+    (1, -2.0, 1.5, 1.5707),
+]
+
+POSITION_TOLERANCE = 0.15  # metres — accounts for one frame of physics settling
+
+
+class _IdleStrategy(AbstractStrategy):
+    """Minimal strategy that does nothing — used when we only care about game state."""
+
+    def create_behaviour_tree(self) -> py_trees.behaviour.Behaviour:
+        return py_trees.behaviours.Success(name="Idle")
+
+    def assert_exp_robots(self, n_runtime_friendly: int, n_runtime_enemy: int) -> bool:
+        return True
+
+    def assert_exp_goals(self, includes_my_goal_line: bool, includes_opp_goal_line: bool) -> bool:
+        return True
+
+    def get_min_bounding_zone(self) -> Optional[FieldBounds]:
+        return None
+
+
+class _TeleportAccuracyTestManager(AbstractTestManager):
+    n_episodes = 1
+
+    def __init__(self, robot_id: int, target_x: float, target_y: float, target_theta: float):
+        super().__init__()
+        self.robot_id = robot_id
+        self.target_x = target_x
+        self.target_y = target_y
+        self.target_theta = target_theta
+        self.first_frame_position = None
+
+    def reset_field(self, sim_controller: AbstractSimController, game: Game):
+        # Park unused robots in-field (bottom-left corner, spaced apart so all are visible)
+        for rid in game.friendly_robots:
+            if rid != self.robot_id:
+                sim_controller.teleport_robot(game.my_team_is_yellow, rid, -4.0, -2.0 + rid * 0.5)
+        # Place target robot at desired position
+        sim_controller.teleport_robot(
+            game.my_team_is_yellow,
+            self.robot_id,
+            self.target_x,
+            self.target_y,
+            self.target_theta,
+        )
+        # Ball must be in-bounds so GameGater can complete
+        sim_controller.teleport_ball(0.0, 0.0)
+
+    def eval_status(self, game: Game) -> TestingStatus:
+        if self.first_frame_position is None:
+            robot = game.friendly_robots.get(self.robot_id)
+            if robot is not None:
+                self.first_frame_position = (robot.p.x, robot.p.y)
+        return TestingStatus.SUCCESS
+
+
+def _run_teleport_accuracy_test(robot_id: int, x: float, y: float, theta: float):
+    n_robots = max(robot_id + 1, 1)
+    test_manager = _TeleportAccuracyTestManager(robot_id, x, y, theta)
+
+    runner = StrategyRunner(
+        strategy=_IdleStrategy(),
+        my_team_is_yellow=True,
+        my_team_is_right=False,
+        mode="rsim",
+        exp_friendly=n_robots,
+        exp_enemy=0,
+    )
+
+    passed = runner.run_test(
+        test_manager=test_manager,
+        episode_timeout=5.0,
+        rsim_headless=True,
+    )
+
+    print("hello!")
+    assert passed, "Test episode did not complete."
+    print("hello!")
+    assert test_manager.first_frame_position is not None, f"Robot {robot_id} was never seen in the first game frame."
+
+    actual_x, actual_y = test_manager.first_frame_position
+    assert abs(actual_x - x) <= POSITION_TOLERANCE, (
+        f"Robot {robot_id} first-frame x={actual_x:.3f} deviates from teleport target x={x:.3f} "
+        f"by {abs(actual_x - x):.3f}m (tolerance={POSITION_TOLERANCE}m)"
+    )
+    assert abs(actual_y - y) <= POSITION_TOLERANCE, (
+        f"Robot {robot_id} first-frame y={actual_y:.3f} deviates from teleport target y={y:.3f} "
+        f"by {abs(actual_y - y):.3f}m (tolerance={POSITION_TOLERANCE}m)"
+    )
+
+
+@pytest.mark.parametrize("rid,x,y,theta", _TELEPORT_CASES)
+def test_first_frame_reflects_teleported_position(rid: int, x: float, y: float, theta: float):
+    """First game frame after reset_field must show the robot at its teleported position."""
+    _run_teleport_accuracy_test(rid, x, y, theta)
+
+
+def test_teleport_ball_first_frame():
+    """First game frame after reset_field must show the ball at its teleported position."""
+    target_bx, target_by = 1.5, -1.0
+    first_ball_pos = {}
+
+    class _BallTeleportManager(AbstractTestManager):
+        n_episodes = 1
+
+        def reset_field(self, sim_controller: AbstractSimController, game: Game):
+            # Park robot 0 in-bounds away from the ball target
+            sim_controller.teleport_robot(game.my_team_is_yellow, 0, -4.0, -2.0, 0.0)
+            sim_controller.teleport_ball(target_bx, target_by)
+
+        def eval_status(self, game: Game) -> TestingStatus:
+            if "pos" not in first_ball_pos and game.ball is not None:
+                first_ball_pos["pos"] = (game.ball.p.x, game.ball.p.y)
+            return TestingStatus.SUCCESS
+
+    runner = StrategyRunner(
+        strategy=_IdleStrategy(),
+        my_team_is_yellow=True,
+        my_team_is_right=False,
+        mode="rsim",
+        exp_friendly=1,
+        exp_enemy=0,
+    )
+
+    passed = runner.run_test(
+        test_manager=_BallTeleportManager(),
+        episode_timeout=5.0,
+        rsim_headless=True,
+    )
+
+    assert passed
+    assert "pos" in first_ball_pos, "Ball was never seen in the first game frame."
+
+    actual_bx, actual_by = first_ball_pos["pos"]
+    assert (
+        abs(actual_bx - target_bx) <= POSITION_TOLERANCE
+    ), f"Ball first-frame x={actual_bx:.3f} deviates from teleport target x={target_bx:.3f}"
+    assert (
+        abs(actual_by - target_by) <= POSITION_TOLERANCE
+    ), f"Ball first-frame y={actual_by:.3f} deviates from teleport target y={target_by:.3f}"

From 12d788415010e451d26c2642bab62c0e72106134 Mon Sep 17 00:00:00 2001
From: Joel <nzmjoel@gmail.com>
Date: Wed, 25 Feb 2026 23:33:06 +0000
Subject: [PATCH 3/9] clean up movement tests

---
 .../motion_planning/multiple_robots_test.py   | 47 ++-------------
 .../motion_planning/random_movement_test.py   | 39 +++----------
 .../single_robot_moving_obstacle_test.py      | 57 ++++++-------------
 .../single_robot_static_obstacle_test.py      | 51 ++++-------------
 .../strategies/random_movement_strategy.py    |  2 +-
 5 files changed, 39 insertions(+), 157 deletions(-)

diff --git a/utama_core/tests/motion_planning/multiple_robots_test.py b/utama_core/tests/motion_planning/multiple_robots_test.py
index 27287a4b..377f37b0 100644
--- a/utama_core/tests/motion_planning/multiple_robots_test.py
+++ b/utama_core/tests/motion_planning/multiple_robots_test.py
@@ -45,50 +45,13 @@ def __init__(self, scenario: MultiRobotScenario):
 
     def reset_field(self, sim_controller: AbstractSimController, game: Game):
         """Reset field with all robots at their starting positions."""
-        # Teleport friendly robots to starting positions
         for i, (x, y) in enumerate(self.scenario.friendly_positions):
-            if i < MAX_ROBOTS:  # Max MAX_ROBOTS robots per team
-                sim_controller.teleport_robot(
-                    game.my_team_is_yellow,
-                    i,
-                    x,
-                    y,
-                    0.0,
-                )
-
-        # Teleport remaining friendly robots far away
-        for i in range(len(self.scenario.friendly_positions), MAX_ROBOTS):
-            sim_controller.teleport_robot(
-                game.my_team_is_yellow,
-                i,
-                -10.0,
-                -10.0,
-                0.0,
-            )
-
-        # Teleport enemy robots to starting positions
+            sim_controller.teleport_robot(game.my_team_is_yellow, i, x, y, 0.0)
+
         for i, (x, y) in enumerate(self.scenario.enemy_positions):
-            if i < MAX_ROBOTS:
-                sim_controller.teleport_robot(
-                    not game.my_team_is_yellow,
-                    i,
-                    x,
-                    y,
-                    0.0,
-                )
-
-        # Teleport remaining enemy robots far away
-        for i in range(len(self.scenario.enemy_positions), MAX_ROBOTS):
-            sim_controller.teleport_robot(
-                not game.my_team_is_yellow,
-                i,
-                -10.0,
-                -10.0,
-                0.0,
-            )
-
-        # Place ball out of the way
-        sim_controller.teleport_ball(-10.0, -10.0)
+            sim_controller.teleport_robot(not game.my_team_is_yellow, i, x, y, 0.0)
+
+        sim_controller.teleport_ball(0.0, 0.0)
 
         self._reset_metrics()
 
diff --git a/utama_core/tests/motion_planning/random_movement_test.py b/utama_core/tests/motion_planning/random_movement_test.py
index c3afb3c7..85a81426 100644
--- a/utama_core/tests/motion_planning/random_movement_test.py
+++ b/utama_core/tests/motion_planning/random_movement_test.py
@@ -52,41 +52,16 @@ def reset_field(self, sim_controller: AbstractSimController, game: Game):
         """Reset field with robots in random starting positions within bounds."""
         (min_x, max_x), (min_y, max_y) = self.scenario.field_bounds
 
-        # Place robots at random positions within bounds
+        # Use a fixed seed for reproducibility across test runs
+        rng = random.Random(42 + self.current_episode_number)
+
         for i in range(self.scenario.n_robots):
-            x = random.uniform(min_x + 0.5, max_x - 0.5)
-            y = random.uniform(min_y + 0.5, max_y - 0.5)
-            sim_controller.teleport_robot(
-                game.my_team_is_yellow,
-                i,
-                x,
-                y,
-                0.0,
-            )
+            x = rng.uniform(min_x + 0.5, max_x - 0.5)
+            y = rng.uniform(min_y + 0.5, max_y - 0.5)
+            sim_controller.teleport_robot(game.my_team_is_yellow, i, x, y, 0.0)
             self.targets_reached_count[i] = 0
 
-        # Teleport remaining robots far away
-        for i in range(self.scenario.n_robots, 6):
-            sim_controller.teleport_robot(
-                game.my_team_is_yellow,
-                i,
-                -10.0,
-                -10.0,
-                0.0,
-            )
-
-        # Remove enemy robots
-        for i in range(6):
-            sim_controller.teleport_robot(
-                not game.my_team_is_yellow,
-                i,
-                -10.0,
-                -10.0,
-                0.0,
-            )
-
-        # Place ball out of the way
-        sim_controller.teleport_ball(-10.0, -10.0)
+        sim_controller.teleport_ball(0.0, 0.0)
 
         self._reset_metrics()
 
diff --git a/utama_core/tests/motion_planning/single_robot_moving_obstacle_test.py b/utama_core/tests/motion_planning/single_robot_moving_obstacle_test.py
index 06695b4b..35ff8353 100644
--- a/utama_core/tests/motion_planning/single_robot_moving_obstacle_test.py
+++ b/utama_core/tests/motion_planning/single_robot_moving_obstacle_test.py
@@ -57,47 +57,22 @@ def __init__(self, scenario: MovingObstacleScenario, robot_id: int):
 
     def reset_field(self, sim_controller: AbstractSimController, game: Game):
         """Reset field with robot at start position and moving obstacles."""
-        # Teleport ALL friendly robots off-field
-        for i in range(6):
-            if i == self.robot_id:
-                # Place the test robot at start position
-                sim_controller.teleport_robot(
-                    game.my_team_is_yellow,
-                    i,
-                    self.scenario.start_position[0],
-                    self.scenario.start_position[1],
-                    0.0,
-                )
-            else:
-                # Move other friendly robots far away
-                sim_controller.teleport_robot(
-                    game.my_team_is_yellow,
-                    i,
-                    -10.0,
-                    -10.0,
-                    0.0,
-                )
-
-        # Place enemy robots at their center positions (they will start moving via their strategy)
-        for i in range(6):
-            if i < len(self.scenario.moving_obstacles):
-                obstacle_config = self.scenario.moving_obstacles[i]
-                sim_controller.teleport_robot(
-                    not game.my_team_is_yellow,
-                    i,
-                    obstacle_config.center_position[0],
-                    obstacle_config.center_position[1],
-                    0.0,
-                )
-            else:
-                # Move extra enemy robots far away
-                sim_controller.teleport_robot(
-                    not game.my_team_is_yellow,
-                    i,
-                    -10.0,
-                    -10.0,
-                    0.0,
-                )
+        sim_controller.teleport_robot(
+            game.my_team_is_yellow,
+            self.robot_id,
+            self.scenario.start_position[0],
+            self.scenario.start_position[1],
+            0.0,
+        )
+
+        for i, obstacle_config in enumerate(self.scenario.moving_obstacles):
+            sim_controller.teleport_robot(
+                not game.my_team_is_yellow,
+                i,
+                obstacle_config.center_position[0],
+                obstacle_config.center_position[1],
+                0.0,
+            )
 
         # Place ball at target (for visual reference)
         sim_controller.teleport_ball(
diff --git a/utama_core/tests/motion_planning/single_robot_static_obstacle_test.py b/utama_core/tests/motion_planning/single_robot_static_obstacle_test.py
index 9ee3425a..164541ef 100644
--- a/utama_core/tests/motion_planning/single_robot_static_obstacle_test.py
+++ b/utama_core/tests/motion_planning/single_robot_static_obstacle_test.py
@@ -42,47 +42,16 @@ def __init__(self, scenario: CollisionAvoidanceScenario, robot_id: int):
 
     def reset_field(self, sim_controller: AbstractSimController, game: Game):
         """Reset field with robot at start position and obstacles along the path."""
-        # Teleport ALL friendly robots off-field first (to clean up from previous tests)
-        for i in range(MAX_ROBOTS):
-            if i == self.robot_id:
-                # Place the test robot at start position
-                sim_controller.teleport_robot(
-                    game.my_team_is_yellow,
-                    i,
-                    self.scenario.start_position[0],
-                    self.scenario.start_position[1],
-                    0.0,  # Face forward
-                )
-            else:
-                # Move other friendly robots far away
-                sim_controller.teleport_robot(
-                    game.my_team_is_yellow,
-                    i,
-                    -10.0,
-                    -10.0,
-                    0.0,
-                )
-
-        # Place enemy robots as obstacles
-        for i in range(MAX_ROBOTS):  # Handle all possible enemy robots
-            if i < len(self.scenario.obstacle_positions):
-                x, y = self.scenario.obstacle_positions[i]
-                sim_controller.teleport_robot(
-                    not game.my_team_is_yellow,
-                    i,
-                    x,
-                    y,
-                    0.0,
-                )
-            else:
-                # Move extra enemy robots far away
-                sim_controller.teleport_robot(
-                    not game.my_team_is_yellow,
-                    i,
-                    -10.0,
-                    -10.0,
-                    0.0,
-                )
+        sim_controller.teleport_robot(
+            game.my_team_is_yellow,
+            self.robot_id,
+            self.scenario.start_position[0],
+            self.scenario.start_position[1],
+            0.0,
+        )
+
+        for i, (x, y) in enumerate(self.scenario.obstacle_positions):
+            sim_controller.teleport_robot(not game.my_team_is_yellow, i, x, y, 0.0)
 
         # Place ball at target (for visual reference)
         sim_controller.teleport_ball(
diff --git a/utama_core/tests/motion_planning/strategies/random_movement_strategy.py b/utama_core/tests/motion_planning/strategies/random_movement_strategy.py
index 2ddc9780..1555d748 100644
--- a/utama_core/tests/motion_planning/strategies/random_movement_strategy.py
+++ b/utama_core/tests/motion_planning/strategies/random_movement_strategy.py
@@ -161,7 +161,7 @@ def __init__(
 
     def assert_exp_robots(self, n_runtime_friendly: int, n_runtime_enemy: int):
         """Requires number of friendly robots to match."""
-        return n_runtime_friendly >= self.n_robots
+        return n_runtime_friendly == self.n_robots
 
     def assert_exp_goals(self, includes_my_goal_line: bool, includes_opp_goal_line: bool):
         return True

From 11dd7a34487da6e316b3268bd934a45da2364aa4 Mon Sep 17 00:00:00 2001
From: Joel <nzmjoel@gmail.com>
Date: Wed, 25 Feb 2026 23:39:12 +0000
Subject: [PATCH 4/9] default filter to false

---
 utama_core/run/strategy_runner.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/utama_core/run/strategy_runner.py b/utama_core/run/strategy_runner.py
index be668ffb..2ec8576d 100644
--- a/utama_core/run/strategy_runner.py
+++ b/utama_core/run/strategy_runner.py
@@ -131,7 +131,7 @@ def __init__(
         profiler_name: Optional[str] = None,
         rsim_noise: RsimGaussianNoise = RsimGaussianNoise(),
         rsim_vanishing: float = 0,
-        filtering: bool = True,
+        filtering: bool = False,
     ):
         self.logger = logging.getLogger(__name__)
 

From d575e28570b6829722dfbe0ada709c2331a9884d Mon Sep 17 00:00:00 2001
From: Joel <nzmjoel@gmail.com>
Date: Wed, 25 Feb 2026 23:45:38 +0000
Subject: [PATCH 5/9] remove conflict in teleport positions

---
 .../tests/strategy_runner/teleport_position_accuracy_test.py    | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/utama_core/tests/strategy_runner/teleport_position_accuracy_test.py b/utama_core/tests/strategy_runner/teleport_position_accuracy_test.py
index 0a6f5dea..0597217a 100644
--- a/utama_core/tests/strategy_runner/teleport_position_accuracy_test.py
+++ b/utama_core/tests/strategy_runner/teleport_position_accuracy_test.py
@@ -22,7 +22,7 @@
     # (rid, x, y, theta)
     (0, 1.0, 1.0, 0.0),
     (0, -1.0, -1.0, 1.5707),
-    (0, 0.0, 0.0, 3.1415),
+    (0, 3.0, 3.0, 3.1415),
     (1, 2.0, -1.5, 0.0),
     (1, -2.0, 1.5, 1.5707),
 ]

From 8afd536186655b9a086a89113ee261a569efb574 Mon Sep 17 00:00:00 2001
From: Joel <nzmjoel@gmail.com>
Date: Thu, 26 Feb 2026 11:44:13 +0000
Subject: [PATCH 6/9] remove print artefacts from
 teleport_position_accuracy_test.py

---
 .../tests/strategy_runner/teleport_position_accuracy_test.py    | 2 --
 1 file changed, 2 deletions(-)

diff --git a/utama_core/tests/strategy_runner/teleport_position_accuracy_test.py b/utama_core/tests/strategy_runner/teleport_position_accuracy_test.py
index 0597217a..b47315f8 100644
--- a/utama_core/tests/strategy_runner/teleport_position_accuracy_test.py
+++ b/utama_core/tests/strategy_runner/teleport_position_accuracy_test.py
@@ -100,9 +100,7 @@ def _run_teleport_accuracy_test(robot_id: int, x: float, y: float, theta: float)
         rsim_headless=True,
     )
 
-    print("hello!")
     assert passed, "Test episode did not complete."
-    print("hello!")
     assert test_manager.first_frame_position is not None, f"Robot {robot_id} was never seen in the first game frame."
 
     actual_x, actual_y = test_manager.first_frame_position

From aecd9ea27f5adb5a615e58c3e504468bdc943d98 Mon Sep 17 00:00:00 2001
From: Joel <nzmjoel@gmail.com>
Date: Thu, 26 Feb 2026 11:53:42 +0000
Subject: [PATCH 7/9] update docstring for strategy_runner.py to reflect
 filtering false default

---
 utama_core/run/strategy_runner.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/utama_core/run/strategy_runner.py b/utama_core/run/strategy_runner.py
index 2ec8576d..510ad77c 100644
--- a/utama_core/run/strategy_runner.py
+++ b/utama_core/run/strategy_runner.py
@@ -111,7 +111,7 @@ class StrategyRunner:
             Defaults to 0 for each.
         rsim_vanishing (float, optional): When running in rsim, cause robots and ball to vanish with the given probability.
             Defaults to 0.
-        filtering (bool, optional): Turn on Kalman filtering. Defaults to true.
+        filtering (bool, optional): Turn on Kalman filtering. Defaults to false.
     """
 
     def __init__(

From ab7df4cdc3cc6ea3e506fa0074526eb97ceda1b2 Mon Sep 17 00:00:00 2001
From: Joel <nzmjoel@gmail.com>
Date: Thu, 26 Feb 2026 12:28:25 +0000
Subject: [PATCH 8/9] move bounds check earlier to prevent mismatch of kalman
 filter and gameframe mismatch

---
 .../data_processing/refiners/position.py      | 55 +++++++-----
 .../tests/refiners/camera_combiner_test.py    | 90 ++++++++++++++++++-
 2 files changed, 121 insertions(+), 24 deletions(-)

diff --git a/utama_core/data_processing/refiners/position.py b/utama_core/data_processing/refiners/position.py
index fea4845f..5851bbf5 100644
--- a/utama_core/data_processing/refiners/position.py
+++ b/utama_core/data_processing/refiners/position.py
@@ -1,5 +1,5 @@
 from collections import defaultdict
-from dataclasses import replace
+from dataclasses import dataclass, replace
 from functools import partial
 from typing import Dict, List, Optional, Tuple
 
@@ -31,6 +31,14 @@ def smooth(self, old_angle: float, new_angle: float) -> float:
         return smoothed_angle
 
 
+@dataclass
+class VisionBounds:
+    x_min: float
+    x_max: float
+    y_min: float
+    y_max: float
+
+
 class PositionRefiner(BaseRefiner):
     def __init__(
         self,
@@ -40,11 +48,12 @@ def __init__(
     ):
         # alpha=0 means no change in angle (inf smoothing), alpha=1 means no smoothing
         self.angle_smoother = AngleSmoother(alpha=1)
-        self.x_min = field_bounds.top_left[0] - bounds_buffer  # expand left
-        self.x_max = field_bounds.bottom_right[0] + bounds_buffer  # expand right
-        self.y_min = field_bounds.bottom_right[1] - bounds_buffer  # expand bottom
-        self.y_max = field_bounds.top_left[1] + bounds_buffer  # expand top
-        self.BOUNDS_BUFFER = bounds_buffer
+        self.vision_bounds = VisionBounds(
+            x_min=field_bounds.top_left[0] - bounds_buffer,  # expand left
+            x_max=field_bounds.bottom_right[0] + bounds_buffer,  # expand right
+            y_min=field_bounds.bottom_right[1] - bounds_buffer,  # expand bottom
+            y_max=field_bounds.top_left[1] + bounds_buffer,  # expand top
+        )
 
         # For Kalman filtering and imputing vanished values.
         self.filtering = filtering
@@ -68,7 +77,7 @@ def refine(self, game_frame: GameFrame, data: List[RawVisionData]) -> GameFrame:
 
         # class VisionData: ts: float; yellow_robots: List[VisionRobotData]; blue_robots: List[VisionRobotData]; balls: List[VisionBallData]
         # class VisionRobotData: id: int; x: float; y: float; orientation: float
-        combined_vision_data: VisionData = CameraCombiner().combine_cameras(frames)
+        combined_vision_data: VisionData = CameraCombiner().combine_cameras(frames, bounds=self.vision_bounds)
 
         time_elapsed = combined_vision_data.ts - game_frame.ts
 
@@ -90,7 +99,6 @@ def refine(self, game_frame: GameFrame, data: List[RawVisionData]) -> GameFrame:
                     if y_rbt_id not in yellow_rbt_last_frame:
                         filtered_yellow_robots.append(vision_y_rbt)
                         continue
-
                 filtered_robot = self.kalman_filters_yellow[y_rbt_id].filter_data(
                     vision_y_rbt,  # new measurement
                     yellow_rbt_last_frame[y_rbt_id],  # last frame
@@ -270,12 +278,6 @@ def _combine_single_team_positions(
         friendly: bool,
     ) -> Dict[int, Robot]:
         for robot in vision_robots:
-            new_x, new_y = robot.x, robot.y
-
-            if not (self.x_min <= new_x <= self.x_max and self.y_min <= new_y <= self.y_max):
-                # Out of bounds so ignore this robot
-                continue
-
             if robot.id not in new_game_robots:
                 # At the start of the game, we haven't seen anything yet, so just create a new robot
                 new_game_robots[robot.id] = PositionRefiner._robot_from_vision(robot, is_friendly=friendly)
@@ -318,9 +320,16 @@ def filter_running(self) -> bool:
 
 
 class CameraCombiner:
-    def combine_cameras(self, frames: List[RawVisionData]) -> VisionData:
-        # Now we have access to the game we can do more sophisticated things
-        # Such as ignoring outlier cameras etc
+    def combine_cameras(self, frames: List[RawVisionData], bounds: VisionBounds) -> VisionData:
+        """
+        Combines the vision data from multiple cameras into a single coherent VisionData object.
+        Also, removes any robot detections that are out of the specified bounds.
+        Args:
+            frames (List[RawVisionData]): A list of RawVisionData objects from different cameras.
+            bounds (VisionBounds): The bounds within which to consider vision data for combination.
+        Returns:
+            VisionData: A combined VisionData object containing averaged robot positions and the most confident ball position.
+        """
 
         ts = []
         # maps robot id to list of frames seen for that robot
@@ -331,13 +340,16 @@ def combine_cameras(self, frames: List[RawVisionData]) -> VisionData:
         # Each frame is from a different camera
         for frame_ind, frame in enumerate(frames):
             for yr in frame.yellow_robots:
-                yellow_captured[yr.id].append(yr)
+                if self._bounds_check(yr.x, yr.y, bounds):
+                    yellow_captured[yr.id].append(yr)
 
             for br in frame.blue_robots:
-                blue_captured[br.id].append(br)
+                if self._bounds_check(br.x, br.y, bounds):
+                    blue_captured[br.id].append(br)
 
             for b in frame.balls:
-                balls_captured[frame_ind].append(b)
+                if self._bounds_check(b.x, b.y, bounds):
+                    balls_captured[frame_ind].append(b)
             ts.append(frame.ts)
 
         avg_yellows = list(map(self._avg_robots, yellow_captured.values()))
@@ -386,6 +398,9 @@ def _combine_balls_by_proximity(self, bs: Dict[int, List[RawBallData]]) -> List[
                     combined_balls.append(b)
         return combined_balls
 
+    def _bounds_check(self, x: float, y: float, bounds: VisionBounds) -> bool:
+        return bounds.x_min <= x <= bounds.x_max and bounds.y_min <= y <= bounds.y_max
+
     @staticmethod
     def ball_merge_predicate(b1: RawBallData, b2: RawBallData) -> bool:
         return abs(b1.x - b2.x) + abs(b1.y - b2.y) < BALL_MERGE_THRESHOLD
diff --git a/utama_core/tests/refiners/camera_combiner_test.py b/utama_core/tests/refiners/camera_combiner_test.py
index 6d06d394..feb64132 100644
--- a/utama_core/tests/refiners/camera_combiner_test.py
+++ b/utama_core/tests/refiners/camera_combiner_test.py
@@ -1,6 +1,10 @@
-from utama_core.data_processing.refiners.position import CameraCombiner
+import math
+
+from utama_core.data_processing.refiners.position import CameraCombiner, VisionBounds
 from utama_core.entities.data.raw_vision import RawBallData, RawRobotData, RawVisionData
 
+infinite_bounds = VisionBounds(x_min=-math.inf, x_max=math.inf, y_min=-math.inf, y_max=math.inf)
+
 
 def test_combine_same_robots_produces_same():
     raw_yellow = [RawRobotData(0, -1, -10, 0, 1)]
@@ -9,7 +13,10 @@ def test_combine_same_robots_produces_same():
     raw_vision_data_cam1 = RawVisionData(0, raw_yellow, raw_blue, raw_balls, 0)
     raw_vision_data_cam2 = RawVisionData(0, raw_yellow, raw_blue, raw_balls, 1)
 
-    combined_data = CameraCombiner().combine_cameras([raw_vision_data_cam1, raw_vision_data_cam2])
+    combined_data = CameraCombiner().combine_cameras(
+        [raw_vision_data_cam1, raw_vision_data_cam2],
+        infinite_bounds,
+    )
 
     for i in range(len(combined_data.yellow_robots)):
         assert combined_data.yellow_robots[i].x == raw_yellow[i].x
@@ -29,7 +36,7 @@ def test_combine_with_one_camera_empty():
     raw_vision_data_cam1 = RawVisionData(0, raw_yellow, raw_blue, raw_balls, 0)
     raw_vision_data_cam2 = RawVisionData(0, [], [], [], 1)
 
-    combined_data = CameraCombiner().combine_cameras([raw_vision_data_cam1, raw_vision_data_cam2])
+    combined_data = CameraCombiner().combine_cameras([raw_vision_data_cam1, raw_vision_data_cam2], infinite_bounds)
 
     for i in range(len(combined_data.yellow_robots)):
         assert combined_data.yellow_robots[i].x == raw_yellow[i].x
@@ -56,7 +63,7 @@ def test_combine_proximity_multiple_balls():
 def test_combine_with_both_camera_empty_gives_empty():
     raw_vision_data_cam2 = RawVisionData(0, [], [], [], 1)
     raw_vision_data_cam1 = RawVisionData(0, [], [], [], 0)
-    combined_data = CameraCombiner().combine_cameras([raw_vision_data_cam1, raw_vision_data_cam2])
+    combined_data = CameraCombiner().combine_cameras([raw_vision_data_cam1, raw_vision_data_cam2], infinite_bounds)
 
     assert combined_data.yellow_robots == []
     assert combined_data.blue_robots == []
@@ -71,6 +78,81 @@ def test_combine_filters_low_confidence():
     assert len(CameraCombiner()._combine_balls_by_proximity(balls)) == 2
 
 
+# --- Bounds filtering tests ---
+
+tight_bounds = VisionBounds(x_min=-5.0, x_max=5.0, y_min=-3.5, y_max=3.5)
+
+
+def test_out_of_bounds_yellow_robot_is_filtered():
+    in_bounds = RawRobotData(0, 1.0, 1.0, 0, 1)
+    out_of_bounds = RawRobotData(1, 10.0, 0.0, 0, 1)  # x > x_max
+    frame = RawVisionData(0, [in_bounds, out_of_bounds], [], [], 0)
+
+    result = CameraCombiner().combine_cameras([frame], tight_bounds)
+
+    assert len(result.yellow_robots) == 1
+    assert result.yellow_robots[0].id == 0
+
+
+def test_out_of_bounds_blue_robot_is_filtered():
+    in_bounds = RawRobotData(0, -1.0, -1.0, 0, 1)
+    out_of_bounds = RawRobotData(1, 0.0, 100.0, 0, 1)  # y > y_max
+    frame = RawVisionData(0, [], [in_bounds, out_of_bounds], [], 0)
+
+    result = CameraCombiner().combine_cameras([frame], tight_bounds)
+
+    assert len(result.blue_robots) == 1
+    assert result.blue_robots[0].id == 0
+
+
+def test_out_of_bounds_ball_is_filtered():
+    in_bounds_ball = RawBallData(0.0, 0.0, 0, 1)
+    out_of_bounds_ball = RawBallData(6.0, 0.0, 0, 1)  # x > x_max
+    frame = RawVisionData(0, [], [], [in_bounds_ball, out_of_bounds_ball], 0)
+
+    result = CameraCombiner().combine_cameras([frame], tight_bounds)
+
+    assert len(result.balls) == 1
+    assert result.balls[0].x == 0.0
+
+
+def test_all_robots_out_of_bounds_gives_empty():
+    robots = [RawRobotData(i, 50.0 + i, 50.0, 0, 1) for i in range(3)]
+    frame = RawVisionData(0, robots, robots, [], 0)
+
+    result = CameraCombiner().combine_cameras([frame], tight_bounds)
+
+    assert result.yellow_robots == []
+    assert result.blue_robots == []
+
+
+def test_robot_exactly_on_boundary_is_included():
+    # Boundary is inclusive
+    on_boundary = RawRobotData(0, 5.0, 3.5, 0, 1)  # exactly at x_max, y_max
+    frame = RawVisionData(0, [on_boundary], [], [], 0)
+
+    result = CameraCombiner().combine_cameras([frame], tight_bounds)
+
+    assert len(result.yellow_robots) == 1
+    assert result.yellow_robots[0].x == 5.0
+    assert result.yellow_robots[0].y == 3.5
+
+
+def test_only_in_bounds_camera_contributes_to_average():
+    """When two cameras see the same robot but one detection is out of bounds,
+    only the in-bounds detection should contribute to the averaged position."""
+    in_bounds_detection = RawRobotData(0, 1.0, 1.0, 0, 1)
+    out_of_bounds_detection = RawRobotData(0, 10.0, 10.0, 0, 1)  # same id, OOB
+    cam1 = RawVisionData(0, [in_bounds_detection], [], [], 0)
+    cam2 = RawVisionData(0, [out_of_bounds_detection], [], [], 1)
+
+    result = CameraCombiner().combine_cameras([cam1, cam2], tight_bounds)
+
+    assert len(result.yellow_robots) == 1
+    assert result.yellow_robots[0].x == 1.0
+    assert result.yellow_robots[0].y == 1.0
+
+
 if __name__ == "__main__":
     test_combine_same_robots_produces_same()
     test_combine_with_one_camera_empty()

From 6861c0ee7a83c6547a9f888bec65742abdd6d5a1 Mon Sep 17 00:00:00 2001
From: Joel <nzmjoel@gmail.com>
Date: Thu, 26 Feb 2026 16:06:04 +0000
Subject: [PATCH 9/9] added comment acknowledge the double initialization of
 Game in run_test

---
 utama_core/tests/common/abstract_test_manager.py | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/utama_core/tests/common/abstract_test_manager.py b/utama_core/tests/common/abstract_test_manager.py
index 10025d59..f0b718dc 100644
--- a/utama_core/tests/common/abstract_test_manager.py
+++ b/utama_core/tests/common/abstract_test_manager.py
@@ -49,9 +49,14 @@ def reset_field(self, sim_controller: AbstractSimController, game: Game):
         """
         Method is called at start of each test episode in strategyRunner.run_test().
         Use this to reset position of robots and ball for the next episode.
+
         Args:
             sim_controller (AbstractSimController): The simulation controller to manipulate robot positions.
             game (Game): The current game state.
+
+        Note:
+            This causes run_test to initialise Game twice on the first episode.
+            However, this is necessary to give the test manager access to the initial Game object before the first episode setup.
         """
         ...