From 8576403e41fc84f006c850f587ed364807677395 Mon Sep 17 00:00:00 2001
From: Srikanth6469 <srikanth6469@gmail.com>
Date: Sun, 31 Aug 2025 14:13:22 +0530
Subject: [PATCH 1/3] Fix: corrected simulation state initialization in
 common.py

---
 tools/sim/lib/common.py | 45 +++++++++++++++++++++++++++++++++++++----
 1 file changed, 41 insertions(+), 4 deletions(-)

diff --git a/tools/sim/lib/common.py b/tools/sim/lib/common.py
index 1324932131..3b2b7fdcf5 100644
--- a/tools/sim/lib/common.py
+++ b/tools/sim/lib/common.py
@@ -59,6 +59,11 @@ class SimulatorState:
   @property
   def speed(self):
     return math.sqrt(self.velocity.x ** 2 + self.velocity.y ** 2 + self.velocity.z ** 2)
+  
+sim_state = SimulatorState()
+sim_state.velocity = vec3(3.25, 10.50, 0) 
+sim_state.bearing = 72.51                  
+sim_state.steering_angle = 7.87 
 
 
 class World(ABC):
@@ -83,10 +88,6 @@ class World(ABC):
   def read_state(self):
     pass
 
-  @abstractmethod
-  def read_sensors(self, simulator_state: SimulatorState):
-    pass
-
   @abstractmethod
   def read_cameras(self):
     pass
@@ -98,3 +99,39 @@ class World(ABC):
   @abstractmethod
   def reset(self):
     pass
+  def read_sensors(self, simulator_state: SimulatorState):
+    """
+    Populate IMUState from simulator kinematics.
+    - accelerometer: m/s^2 (Δv/Δt)
+    - gyroscope: rad/s (yaw rate from Δbearing)
+    - bearing: radians (absolute yaw)
+    """
+    import time
+    now = time.monotonic()
+    imu = simulator_state.imu
+
+    v = simulator_state.velocity or vec3(0.0, 0.0, 0.0)
+    bearing_rad = math.radians(simulator_state.bearing or 0.0)
+
+    prev = getattr(self, "_imu_prev", None)
+    if prev is None:
+        imu.accelerometer = vec3(0.0, 0.0, 0.0)
+        imu.gyroscope = vec3(0.0, 0.0, 0.0)
+    else:
+        dt = max(1e-3, now - prev["t"])  
+        dvx = v.x - prev["vx"]
+        dvy = v.y - prev["vy"]
+        dvz = v.z - prev["vz"]
+
+        ax = dvx / dt
+        ay = dvy / dt
+        az = dvz / dt
+        db = (bearing_rad - prev["bearing"] + math.pi) % (2 * math.pi) - math.pi
+        yaw_rate = db / dt
+
+        imu.accelerometer = vec3(ax, ay, az)
+        imu.gyroscope = vec3(0.0, 0.0, yaw_rate)
+
+    imu.bearing = bearing_rad
+    self._imu_prev = {"t": now, "vx": v.x, "vy": v.y, "vz": v.z, "bearing": bearing_rad}
+

From f6ed11863e884bab191d863f46f1f32d267f6a15 Mon Sep 17 00:00:00 2001
From: Srikanth6469 <srikanth6469@gmail.com>
Date: Sun, 31 Aug 2025 14:44:38 +0530
Subject: [PATCH 2/3] Fix mypy unreachable warning in IMU update

---
 tools/sim/lib/common.py | 20 +++++---------------
 1 file changed, 5 insertions(+), 15 deletions(-)

diff --git a/tools/sim/lib/common.py b/tools/sim/lib/common.py
index 3b2b7fdcf5..07ec2e2532 100644
--- a/tools/sim/lib/common.py
+++ b/tools/sim/lib/common.py
@@ -1,13 +1,10 @@
 import math
 import multiprocessing
 import numpy as np
-
 from abc import ABC, abstractmethod
 from collections import namedtuple
 
 W, H = 1928, 1208
-
-
 vec3 = namedtuple("vec3", ["x", "y", "z"])
 
 class GPSState:
@@ -59,21 +56,19 @@ class SimulatorState:
   @property
   def speed(self):
     return math.sqrt(self.velocity.x ** 2 + self.velocity.y ** 2 + self.velocity.z ** 2)
-  
+    
 sim_state = SimulatorState()
 sim_state.velocity = vec3(3.25, 10.50, 0) 
-sim_state.bearing = 72.51                  
-sim_state.steering_angle = 7.87 
+sim_state.bearing = 72.51
+sim_state.steering_angle = 7.87
 
 
 class World(ABC):
   def __init__(self, dual_camera):
     self.dual_camera = dual_camera
-
     self.image_lock = multiprocessing.Semaphore(value=0)
     self.road_image = np.zeros((H, W, 3), dtype=np.uint8)
     self.wide_road_image = np.zeros((H, W, 3), dtype=np.uint8)
-
     self.exit_event = multiprocessing.Event()
 
   @abstractmethod
@@ -109,10 +104,8 @@ class World(ABC):
     import time
     now = time.monotonic()
     imu = simulator_state.imu
-
-    v = simulator_state.velocity or vec3(0.0, 0.0, 0.0)
-    bearing_rad = math.radians(simulator_state.bearing or 0.0)
-
+    v = simulator_state.velocity if simulator_state.velocity is not None else vec3(0.0, 0.0, 0.0)
+    bearing_rad = math.radians(simulator_state.bearing)
     prev = getattr(self, "_imu_prev", None)
     if prev is None:
         imu.accelerometer = vec3(0.0, 0.0, 0.0)
@@ -122,16 +115,13 @@ class World(ABC):
         dvx = v.x - prev["vx"]
         dvy = v.y - prev["vy"]
         dvz = v.z - prev["vz"]
-
         ax = dvx / dt
         ay = dvy / dt
         az = dvz / dt
         db = (bearing_rad - prev["bearing"] + math.pi) % (2 * math.pi) - math.pi
         yaw_rate = db / dt
-
         imu.accelerometer = vec3(ax, ay, az)
         imu.gyroscope = vec3(0.0, 0.0, yaw_rate)
-
     imu.bearing = bearing_rad
     self._imu_prev = {"t": now, "vx": v.x, "vy": v.y, "vz": v.z, "bearing": bearing_rad}
 

From ade5f1bf915e8cf2872dc261ee0ed0cbeef5aef9 Mon Sep 17 00:00:00 2001
From: Srikanth6469 <srikanth6469@gmail.com>
Date: Sun, 31 Aug 2025 14:56:44 +0530
Subject: [PATCH 3/3] Fix: cleaned up whitespace and blank lines for CI

---
 tools/sim/lib/common.py | 7 ++-----
 1 file changed, 2 insertions(+), 5 deletions(-)

diff --git a/tools/sim/lib/common.py b/tools/sim/lib/common.py
index 07ec2e2532..6c101c7b8c 100644
--- a/tools/sim/lib/common.py
+++ b/tools/sim/lib/common.py
@@ -56,13 +56,10 @@ class SimulatorState:
   @property
   def speed(self):
     return math.sqrt(self.velocity.x ** 2 + self.velocity.y ** 2 + self.velocity.z ** 2)
-    
 sim_state = SimulatorState()
-sim_state.velocity = vec3(3.25, 10.50, 0) 
+sim_state.velocity = vec3(3.25, 10.50, 0)
 sim_state.bearing = 72.51
 sim_state.steering_angle = 7.87
-
-
 class World(ABC):
   def __init__(self, dual_camera):
     self.dual_camera = dual_camera
@@ -111,7 +108,7 @@ class World(ABC):
         imu.accelerometer = vec3(0.0, 0.0, 0.0)
         imu.gyroscope = vec3(0.0, 0.0, 0.0)
     else:
-        dt = max(1e-3, now - prev["t"])  
+        dt = max(1e-3, now - prev["t"])
         dvx = v.x - prev["vx"]
         dvy = v.y - prev["vy"]
         dvz = v.z - prev["vz"]