Feature/kalman (#101)

* Added extracted vision data with varying degrees of vanishing and noise

* Attempt to store past raw game frames; starting work on test cases

* added Andrew's filter

* Integrated filters into position refiner

* bad example test

* Refined filter weights, added vision data for testing

* Updated test data

* Noise now added manually, updated test data

* Amended test cases to address CI failure due to changes to PositionRefiner's constructor

* Refinements to filters based on empirical data

* Added some references for building tests

* First pass unit tests

* Some issues with unit tests. Working on fix

* Added live testing utilities, conducted analytics for filters, refactored classes, added new datasets

* Finalised analysis of filters, unit tests are working, added more live testing utilities

* Deleted redundant files

* Minor refinements to the live testing utilities

* fixed formatting issues

* Commented out utilities for testing and exporting data

* Analysed vanishing problem, created sample datasets

* Position refiner now has access to the last game frame and command map. Imputing feature is coming along , but there are unexplained spikes that are preventing it from working properly

* Fixed issues, now imputes correctly based on last gameframe

* Completed work on vanishing

* Incorporated some suggestions from copilot regardign style

* Addressed comments on PR

* Removed redundant utility function

* Factored out static method into global utility

* cleanup

* Added back error handling for imports when run from Jupyter

* Renamed datasets to include the control mode, filtering only turned on during run mode together with vanishing

* Fixed bug in tests due to renaming of files

* Created working Kalman filter. Working on analytics

* Working on analytics

* Implemented a flag to add Gaussian noise in rsim. Added another more hackish implementation for comparison.

* Analysing Kalman filter performance; need to merge new rsim utilities first

* Cleaned up implementation, removed hackish alternative

* Working on adding noise to each component independently

* Completed implementation of noise addition

* Addressed comments on PR, got orientation to work

* Performed benchmarking on kalman filters

* Performed benchmarking on kalman filters

* More benchmarking

* Kalman filter performance validated

* Kalman filters now handle vanishing

* Removed non-performing implementations

* Rejigged analytics for Kalman filter

* Addressed comments in PR, added vanishing, added support for ball

* Minor linting

* Merge branch 'feature/rsim_noise' into feature/kalman

* Added filter for ball, added support for orientation

* Now able to run analytics for orientation

* Moved utility functions for adding Gaussian noise from classes representing balls/robots to the SSLStdenv class. Learned about this best practice in this week's lecture

* Turned off a flag used for testing

* Minor bug fix

* Analysing data for orientation, plus some minor fixes

* Completed work with Kalman filters, will now work on PR

* Removed some redundant files

* Renamed folder to ease merging with testing

* Renamed folder to ease merging with testing

* Cleanup, getting ready for PR

* Small fix to satisfy linter

* Final cleanup after reviewing PR diff

* Improved filter docs

* add docstring to math_utils.py

* remove unused get_displacement_vector

* remove unused import

* [chore] formatting

* chore: formatting

* fix matrix inversion numerical instability and formatting of class name

* remove improper static method use

* improper use of tuple[float] type hint and mix bug referencing self.covariance_mat_xy instead of self.covariance_mat in KalmanFilterBall

* removed references of .cmd_map not used position.py

* fix test failure

* Made non-global constant variable lower-cased

* Using matrix transpose attribute instead of function call

* update test cases

* Removed redundant last_game_frame attribute

* separate my and opp refiners

* update running attribute for opposite refiner too

* Minor formatting and naming changes to Kalman filter

* fix incorrect assumption about robot id numbering in position refiner tests

* corrected direct imputation of None values into vision data. Fix bug causing test fail

* improve reset functionaity of position refiner

* Fixed bugs introduced when removing assumption about numbering

* fix dictionary logic in position refiner

* update optional typing for KalmanFilterBall filter

* fix typing in KalmanFilterBall

* fix point cycle strat

* Fixed issues in test due to filters no longer having ids, and now taking Robot instead of Dict[Int, Robot]

* Kalman filter now has id attribute

* label vision data using kalman id instead of last frame id

* add comment about treating substitution as vanishing

* remove unncessary imports

* add authors

---------

Co-authored-by: Fred Huang <fredhuang122106@gmail.com>
Co-authored-by: Joel <nzmjoel@gmail.com>

Author: 3 people

.gitignore

@@ -113,6 +113,7 @@ target/
 
 # Jupyter Notebook
 .ipynb_checkpoints
+.jupyter/
 
 # IPython
 profile_default/

main.py

@@ -5,6 +5,7 @@
 from utama_core.strategy.examples import (
     DefenceStrategy,
     GoToBallExampleStrategy,
+    PointCycleStrategy,
     RobotPlacementStrategy,
     StartupStrategy,
     TwoRobotPlacementStrategy,
@@ -17,15 +18,15 @@ def main():
     custom_bounds = FieldBounds(top_left=(2.25, 1.5), bottom_right=(4.5, -1.5))
 
     runner = StrategyRunner(
-        strategy=TwoRobotPlacementStrategy(first_robot_id=0, second_robot_id=1, field_bounds=custom_bounds),
+        strategy=PointCycleStrategy(n_robots=2, field_bounds=custom_bounds, endpoint_tolerance=0.1, seed=42),
         my_team_is_yellow=True,
         my_team_is_right=True,
         mode="rsim",
         exp_friendly=2,
         exp_enemy=0,
         replay_writer_config=ReplayWriterConfig(replay_name="test_replay", overwrite_existing=True),
         print_real_fps=True,
-        profiler_name=None
+        profiler_name=None,
     )
     runner.my_strategy.render()
     runner.run()

pixi.lock

@@ -10,7 +10,11 @@ authors = [
     { name = "Hamish Starling", email = "hamish.starling22@imperial.ac.uk" },
     { name = "Luke Moran", email = "luke.moran22@imperial.ac.uk" },
     { name = "Sadig Sadikhzada", email = "sadig.sadikhzada23@imperial.ac.uk" },
-    { name = "Li Han", email = "han.li124@imperial.ac.uk" }
+    { name = "Li Han", email = "han.li124@imperial.ac.uk" },
+    { name = "Valentin Bruhl", email = "valentin.bruhl24@imperial.ac.uk" },
+    { name = "Sarthak Agarwal", email = "sarthak.agarwal25@imperial.ac.uk" },
+    { name = "Low Sze Yoong", email = "szeyoong.low25@imperial.ac.uk" },
+    { name = "Jiaming Liu", email = "jiaming.liu25@imperial.ac.uk" },
 ]
 requires-python = ">=3.11"
 

pyproject.toml

@@ -56,7 +56,7 @@ def normalise_heading_deg(angle):
         The normalized angle in the range [-180, 180] degrees.
     """
     half_rev = 180
-    
+
     return (angle + half_rev) % (2 * half_rev) - half_rev
 
 

utama_core/global_utils/math_utils.py

@@ -1,8 +1,9 @@
 import logging
-from numpy.random import normal
 import random
 from typing import List, Tuple
 
+from numpy.random import normal
+
 from utama_core.config.formations import LEFT_START_ONE, RIGHT_START_ONE
 from utama_core.config.robot_params import RSIM_PARAMS
 from utama_core.config.settings import (
@@ -13,11 +14,15 @@
 )
 from utama_core.entities.data.command import RobotResponse
 from utama_core.entities.data.raw_vision import RawBallData, RawRobotData, RawVisionData
-from utama_core.global_utils.math_utils import deg_to_rad, rad_to_deg, normalise_heading_deg
+from utama_core.global_utils.math_utils import (
+    deg_to_rad,
+    normalise_heading_deg,
+    rad_to_deg,
+)
 from utama_core.rsoccer_simulator.src.Entities import Ball, Frame, Robot
 from utama_core.rsoccer_simulator.src.ssl.ssl_gym_base import SSLBaseEnv
-from utama_core.rsoccer_simulator.src.Utils.gaussian_noise import RsimGaussianNoise
 from utama_core.rsoccer_simulator.src.Utils import KDTree
+from utama_core.rsoccer_simulator.src.Utils.gaussian_noise import RsimGaussianNoise
 
 logger = logging.getLogger(__name__)
 
@@ -38,19 +43,19 @@ class SSLStandardEnv(SSLBaseEnv):
         Description:
             list of (x, y, theta) coords for each robot to spawn in (in meters and radians).
             See the default BLUE_START_ONE/YELLOW_START_ONE for reference.
-        
+
         gaussian_noise
         Type: RsimGaussianNoise
         Description:
             When running in rsim, add Gaussian noise to ball and robots with the given standard deviations.
             Mutates the Robot object in place. The 3 parameters are for x (in m), y (in m), and orientation (in degrees)
             respectively. Defaults to 0 for each.
-            
+
         vanishing
         Type: float
         Description:
             When running in rsim, cause robots and ball to vanish with the given probability. Defaults to 0.
-            
+
     Observation:
         Type: Tuple[FrameData, List[RobotInfo], List[RobotInfo]]
         Num     Item
@@ -78,7 +83,7 @@ def __init__(
         blue_starting_formation: list[tuple] = None,
         yellow_starting_formation: list[tuple] = None,
         gaussian_noise: RsimGaussianNoise = RsimGaussianNoise(),
-        vanishing: float = 0
+        vanishing: float = 0,
     ):
         super().__init__(
             field_type=field_type,
@@ -108,11 +113,11 @@ def __init__(
         self.latest_observation = (-1, None)
 
         logger.info(f"{n_robots_blue}v{n_robots_yellow} SSL Environment Initialized")
-        
+
         # Adding Gaussian noise and vanishing. Refer to StrategyRunner
         self.gaussian_noise = gaussian_noise
-        
-        assert vanishing >= 0
+
+        assert vanishing >= 0, "Negative vanishing probability not allowed"
         self.vanishing = vanishing
 
     def reset(self, *, seed=None, options=None):
@@ -172,7 +177,7 @@ def _frame_to_observations(
         yellow_robots_info: feedback from individual yellow robots that returns a List[RobotInfo]
         blue_robots_info: feedback from individual blue robots that returns a List[RobotInfo]
         """
-        
+
         if self.latest_observation[0] == self.steps:
             return self.latest_observation[1]
 
@@ -189,7 +194,7 @@ def _frame_to_observations(
         for i in range(len(self.frame.robots_blue)):
             if self._vanishing():
                 continue
-            
+
             robot = self.frame.robots_blue[i]
             robot_pos, robot_info = self._get_robot_observation(robot)
             blue_obs.append(robot_pos)
@@ -200,7 +205,7 @@ def _frame_to_observations(
         for i in range(len(self.frame.robots_yellow)):
             if self._vanishing():
                 continue
-                
+
             robot = self.frame.robots_yellow[i]
             robot_pos, robot_info = self._get_robot_observation(robot)
             yellow_obs.append(robot_pos)
@@ -221,7 +226,7 @@ def _frame_to_observations(
 
     def _get_robot_observation(self, robot):
         SSLStandardEnv._add_gaussian_noise_robot(robot, self.gaussian_noise)
-        
+
         robot_pos = RawRobotData(robot.id, robot.x, -robot.y, -float(deg_to_rad(robot.theta)), 1)
         robot_info = RobotResponse(robot.id, robot.infrared)
         return robot_pos, robot_info
@@ -415,47 +420,45 @@ def in_gk_area(obj):
 
         return pos_frame
 
-
     def _vanishing(self) -> bool:
         """Determines whether a frame should vanish. Only runs after Game Gater is passed"""
         return self.steps > 0 and self.vanishing and (random.random() < self.vanishing)
 
-    
     @staticmethod
     def _add_gaussian_noise_ball(ball: Ball, noise: RsimGaussianNoise):
         """
         When running in rsim, add Gaussian noise to ball with the given standard deviations.
         Mutates the Robot object in place.
-        
+
         Args:
             noise (RsimGaussianNoise): The 3 parameters are for x (in m), y (in m), and orientation (in degrees) respectively.
                 Defaults to 0 for each.
         """
-        
+
         if noise.x_stddev:
             ball.x += normal(scale=noise.x_stddev)
-            
+
         if noise.y_stddev:
             ball.y += normal(scale=noise.y_stddev)
-            
+
         # No noise addition for z, since rSim is 2-D
-        
+
     @staticmethod
     def _add_gaussian_noise_robot(robot: Robot, noise: RsimGaussianNoise):
         """
         When running in rsim, add Gaussian noise to robot with the given standard deviations.
         Mutates the Robot object in place.
-        
+
         Args:
             noise (RsimGaussianNoise): The 3 parameters are for x (in m), y (in m), and orientation (in degrees) respectively.
                 Defaults to 0 for each.
         """
-        
+
         if noise.x_stddev:
             robot.x += normal(scale=noise.x_stddev)
-            
+
         if noise.y_stddev:
             robot.y += normal(scale=noise.y_stddev)
-            
+
         if noise.th_stddev_deg:
-            robot.theta = normalise_heading_deg(robot.theta + normal(scale=noise.th_stddev_deg))
+            robot.theta = normalise_heading_deg(robot.theta + normal(scale=noise.th_stddev_deg))