Steering offset ukf (#84)

Co-authored-by: Rishi Kumar <rishikumar@cmu.edu>
Co-authored-by: Mehul Goel <mehulgoel873@gmail.com>
Co-authored-by: Saransh Agrawal <saransh323@gmail.com>
Co-authored-by: nuc@short-circut <officers@roboclub.org>
Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
Co-authored-by: Harshal <hpuranik@andrew.cmu.edu>
Co-authored-by: TiaSinghania <samanvita.singhania@gmail.com>

Author: 8 people

.gitignore

@@ -19,3 +19,4 @@ vision/data/
 #VENV
 .sc/
 .nand/
+.venv/

rb_ws/src/buggy/config/nand-roll.yaml

@@ -13,8 +13,16 @@ NAND:
       stateTopic: "self/state"
       trajectoryTopic: "self/cur_traj"
       steeringTopic: "input/steering"
+      steerOffsetTopic: "self/steering_offset/filtered"
       useHeadingRate: true
       controllerName: "controller"
       useSteerOffset: false
       # traj_name: "buggycourse_safe.json"
       controller: "stanley"
+      useSteerOffset: false
+
+  NAND_offset_estimator:
+    ros__parameters:
+      steerOffsetRawTopic: "self/steering_offset/raw"
+      steerOffsetFilteredTopic: "self/steering_offset/filtered"
+      steerOffsetFilterTimeConstant: 50   # in timesteps; smoothing factor alpha for low pass filter is 1 / steerOffsetFilterTimeConstant

rb_ws/src/buggy/config/sc-roll.yaml

@@ -13,11 +13,19 @@ SC:
       stateTopic: "self/state"
       trajectoryTopic: "self/cur_traj"
       steeringTopic: "input/steering"
+      steerOffsetTopic: "self/steering_offset/filtered"
       useHeadingRate: true
       controllerName: "controller"
       useSteerOffset: false 
       # traj_name: "buggycourse_safe.json"
       controller: "stanley"
+      useSteerOffset: false 
+
+  SC_offset_estimator:
+    ros__parameters:
+      steerOffsetRawTopic: "self/steering_offset/raw"
+      steerOffsetFilteredTopic: "self/steering_offset/filtered"
+      steerOffsetFilterTimeConstant: 50   # in timesteps; smoothing factor alpha for low pass filter is 1 / steerOffsetFilterTimeConstant
 
   SC_NAND_controller:
     ros__parameters:
@@ -37,4 +45,4 @@ SC:
 
   SC_vision:
     ros__parameters:
-      model_name: "01-15-25_no_pushbar_yolov11n.pt"
+      model_name: "01-15-25_no_pushbar_yolov11n.pt"

rb_ws/src/buggy/config/sim_single_sc.yaml

@@ -1,17 +1,26 @@
 SC:
   SC_sim_single:
     ros__parameters:
+      pose: 'Hill1_SC'
       velocity: 12.0
-      steering_offset: 8.0 # between 20.0 and -20.0, in degrees
-      steering_offset_func: 'sin' # 'constant' or 'sin'
+      steering_offset: 4.0 # between 20.0 and -20.0, in degrees
+      steering_offset_func: 'constant' # 'constant' or 'sin'
       steering_offset_period: 10.0 # seconds, only used when steering_offset_func is 'sin'
       steering_delay: 0 # 1 step = 0.01s (at 100 hz steering)
-      step_noise_std: 0.05 # standard deviation of simulated GPS noise (in meters) 
-      pose: 'Hill1_SC'
+
+      # noise compounds at each timestep, note: square root of time rule applies
+      process_noise_std: 0.001 # standard deviation of process GPS noise (in meters)
+      measurement_noise_std: 0.02 # standard deviation of measured GPS noise (in meters)
 
   SC_controller:
     ros__parameters:
       dist: 0.0
       traj_name: 'buggycourse_sc.json'
       controller: 'stanley'
-      useSteerOffset: false 
+      useSteerOffset: true
+
+  SC_offset_estimator:
+    ros__parameters:
+      steerOffsetRawTopic: "self/steering_offset/raw"
+      steerOffsetFilteredTopic: "self/steering_offset/filtered"
+      steerOffsetFilterTimeConstant: 50

rb_ws/src/buggy/launch/nand-main.xml

@@ -1,7 +1,10 @@
 <launch>
     <arg name="config_file" default="src/buggy/config/nand-roll.yaml"/>
     <node pkg="buggy" exec="controller_node.py" name="NAND_controller" output = "screen" namespace="NAND">
-            <param from="$(var config_file)"/>
+        <param from="$(var config_file)"/>
+    </node>
+    <node pkg="buggy" exec="steer_offset_estimator.py" name="NAND_offset_estimator" namespace="NAND">
+        <param from="$(var config_file)"/>
     </node>
     <node pkg="buggy" exec="steer_offset_estimator.py" name="NAND_offset_estimator" namespace="NAND"/>
 </launch>

rb_ws/src/buggy/launch/sc-main.xml

@@ -6,7 +6,9 @@
     <node pkg="buggy" exec="path_planner.py" name="SC_path_planner" namespace="SC" output = "screen">
         <param from="$(var config_file)"/>
     </node>
-    <node pkg="buggy" exec="steer_offset_estimator.py" name="SC_offset_estimator" namespace="SC"/>
+    <node pkg="buggy" exec="steer_offset_estimator.py" name="SC_offset_estimator" namespace="SC">
+        <param from="$(var config_file)"/>
+    </node>
 
     <node pkg="buggy" exec="controller_node.py" name="SC_NAND_controller" output = "screen" namespace="SC">
         <param from="$(var config_file)"/>

rb_ws/src/buggy/launch/sim_2d_single.xml

@@ -11,7 +11,9 @@
     <node pkg="buggy" exec="controller_node.py" name="SC_controller" namespace="SC">
         <param from="$(var config_file)"/>
     </node>
-    <node pkg="buggy" exec="steer_offset_estimator.py" name="SC_offset_estimator" namespace="SC"/>
+    <node pkg="buggy" exec="steer_offset_estimator.py" name="SC_offset_estimator" namespace="SC">
+        <param from="$(var config_file)"/>
+    </node>
 
     <!-- # This is the new velocity-updated code (commented out for now in case it broke something)
 

rb_ws/src/buggy/scripts/controller/controller_node.py

@@ -25,32 +25,26 @@ def __init__(self):
         super().__init__('controller')
         self.get_logger().info('INITIALIZED.')
 
-
-        #Parameters
-        self.declare_parameter("dist", 0.0) #Starting Distance along path
+        # Parameters
+        self.declare_parameter("dist", 0.0) # Starting Distance along path
         start_dist = self.get_parameter("dist").value
 
-        self.declare_parameter("controllerName", "controller")
-
-        self.declare_parameter("traj_name", "buggycourse_safe.json")
-        traj_name = self.get_parameter("traj_name").value
-        self.cur_traj = Trajectory(json_filepath=os.environ["TRAJPATH"] + traj_name)
-
         self.declare_parameter("stateTopic", "self/state")
         self.declare_parameter("steeringTopic", "input/steering")
         self.declare_parameter("rawSteeringTopic", "input/steering_raw")
         self.declare_parameter("trajectoryTopic", "self/cur_traj")
-        self.declare_parameter("offsetTopic", "self/steer_offset")
+        self.declare_parameter("steerOffsetTopic", "self/steering_offset/filtered")
         self.declare_parameter("useSteerOffset", False)
         self.use_steer_offset = self.get_parameter("useSteerOffset").value
 
+        self.declare_parameter("traj_name", "buggycourse_safe.json")
+        traj_name = self.get_parameter("traj_name").value
+        self.cur_traj = Trajectory(json_filepath=os.environ["TRAJPATH"] + traj_name)
         start_index = self.cur_traj.get_index_from_distance(start_dist)
-
-        self.declare_parameter("controller", "stanley")
-
-
         self.declare_parameter("useHeadingRate", True)
 
+        self.declare_parameter("controllerName", "controller")
+        self.declare_parameter("controller", "stanley")
         controller_name = self.get_parameter("controller").value
         print(controller_name.lower())
         if (controller_name.lower() == "stanley"):
@@ -78,7 +72,7 @@ def __init__(self):
         # Subscribers
         self.odom_subscriber = self.create_subscription(Odometry, self.get_parameter("stateTopic").value, self.odom_listener, 1)
         self.traj_subscriber = self.create_subscription(TrajectoryMsg, self.get_parameter("trajectoryTopic").value, self.traj_listener, 1)
-        self.steer_offset_subscriber = self.create_subscription(Float64, self.get_parameter("offsetTopic").value, self.offset_listener, 1)
+        self.steer_offset_subscriber = self.create_subscription(Float64, self.get_parameter("steerOffsetTopic").value, self.offset_listener, 1)
 
         self.odom = None
         self.passed_init = False
@@ -170,7 +164,6 @@ def loop(self):
         self.steer_publisher.publish(StampedFloat64Msg(header=odom.header, data=float(steering_angle_deg.item())))
 
 
-
 def main(args=None):
     rclpy.init(args=args)
 

rb_ws/src/buggy/scripts/estimation/nand_estimator.py

@@ -8,6 +8,7 @@
 from nav_msgs.msg import Odometry
 from buggy.msg import StampedFloat64Msg
 
+from util.constants import Constants
 
 from ukf_utils import *
 
@@ -90,7 +91,7 @@ def __init__(self):
         self.start = False
 
         self.x_hat = None
-        self.Sigma = np.diag([1e-4, 1e-4, 1e-2, 1e-2]) #state covariance
+        self.Sigma = np.diag([1e-4, 1e-4, 1e-2, 1e-2]) # state covariance
         self.R = self.accuracy_to_mat(50)
         self.Q = np.diag([1e-4, 1e-4, 1e-2, 2.4e-1])
 
@@ -148,17 +149,41 @@ def loop(self):
 
     def accuracy_to_mat(self, accuracy):
         """
-        Convert a scalar accuracy to a 2x2 position covariance matrix.
+        Convert a scalar *circular* position accuracy to a 2x2 position covariance matrix.
 
         Args:
-            accuracy: Position accuracy.
+            accuracy: Position accuracy (radius) in millimeters.
+
+        We assume the position error in x and y follows a 2D Gaussian
+        (normal) distribution that is:
+        * centered at zero (no bias)
+        * has the same spread in x and y
+        * has no correlation between x and y
+
+        The constant 0.848867684498 is a precomputed factor that relates
+        this circular accuracy radius to the underlying standard deviation 
+        of that Gaussian.
+
+        Once we have that standard deviation σ (in meters), the covariance
+        matrix for (x, y) is simply:
+        [ σ²  0  ]
+        [ 0   σ² ]
 
         Returns:
-            2x2 diagonal covariance matrix for x/y position.
+            2x2 diagonal covariance matrix for x/y position (in m²).
         """
+        # convert radius from millimeters to meters
         accuracy /= 1000.0
-        sigma = (accuracy / (0.848867684498)) * (accuracy / (0.848867684498))
-        return np.diag([sigma, sigma])
+
+        # accuracy is the circular error radius (meters)
+        # k is the factor that maps between this radius and the Gaussian σ
+        k = Constants.CEP50_to_STD
+        sigma = accuracy / k
+
+        # variance in each axis (assuming isotropic uncertainty)
+        sigma_sq = sigma * sigma
+
+        return np.diag([sigma_sq, sigma_sq])
 
 def main(args=None):
     rclpy.init(args=args)

rb_ws/src/buggy/scripts/estimation/steer_offset_estimator.py

@@ -12,6 +12,7 @@
 
 from estimation import ukf_utils
 from util.constants import Constants
+from util.LowPassFilter import LowPassFilter
 
 class SteerOffsetEstimator(Node):
     """
@@ -95,15 +96,15 @@ def __init__(self):
         self.enabled = True  # estimator enabled
         self.auton_enabled_prev = None  # previous auton flag for edge detection
 
-        # moved to reset_filter()
-        # self.x_hat: np.ndarray = None
-        # self.Sigma: np.ndarray = np.diag([1e-4, 1e-4, 1e-2, 1e-2, 1.2e-3]) # state covariance
-        # self.Q = np.diag([1e-4, 1e-4, 1e-4, 2.4e-1, 1e-6])  # init process covariance values (2.4e-1 for velocity based on 3 x std dev of 0.16)
-        # self.R = np.diag([1e-2, 1e-2])  # init sensor covariance values
-        # self.last_time = None
-
         self.reset_filter() # initialize filter state
 
+        self.declare_parameter("steerOffsetFilterTimeConstant", 50)
+        steerOffsetFilterTimeConstant = self.get_parameter("steerOffsetFilterTimeConstant").value
+        self.lowPassFilter = LowPassFilter(alpha = 1.0 / steerOffsetFilterTimeConstant)
+
+        self.declare_parameter("steerOffsetRawTopic", "self/steer_offset/raw")
+        self.declare_parameter("steerOffsetFilteredTopic", "self/steer_offset/filtered")
+
         if (self.get_namespace() == "/SC"):
             self.wheelbase = Constants.WHEELBASE_SC
             self.create_subscription(SCDebugInfoMsg, "debug/firmware", self.firmware_debug_callback, 1)
@@ -112,10 +113,13 @@ def __init__(self):
             self.create_subscription(NANDDebugInfoMsg, "debug/firmware", self.firmware_debug_callback, 1)
         self.create_subscription(Odometry, "self/state", self.update_measurement, 1) # Using EKF output for simplicity
         self.create_subscription(StampedFloat64Msg, "input/steering", self.update_steering, 1)
-        self.offset_publisher = self.create_publisher(Float64, "self/steer_offset", 1)
+
+        self.offset_publisher_raw = self.create_publisher(Float64, self.get_parameter("steerOffsetRawTopic").value, 1)
+        self.offset_publisher_filtered = self.create_publisher(Float64, self.get_parameter("steerOffsetFilteredTopic").value, 1)
         self.state_publisher = self.create_publisher(Float64MultiArray, "self/offset_estimator/state", 1)
         self.state_covar_publisher = self.create_publisher(Float64MultiArray, "self/offset_estimator/covariance", 1)
 
+
         self.steering = 0
 
         self.timer = self.create_timer(0.01, self.loop)
@@ -124,8 +128,10 @@ def reset_filter(self):
         """Reset the UKF so next measurement initializes state."""
         self.start = False
         self.x_hat: np.ndarray = np.zeros((5,))  # state vector
-        self.Sigma: np.ndarray = np.diag([1e-4, 1e-4, 1e-2, 1e-2, 1.2e-3]) # state covariance
-        self.Q = np.diag([1e-4, 1e-4, 1e-4, 2.4e-1, 1e-6])  # init process covariance values (2.4e-1 for velocity based on 3 x std dev of 0.16)
+
+        # Offset variance extremely high out of caution and proof of convergence
+        self.Sigma: np.ndarray = np.diag([1e-4, 1e-4, 1e-2, 1e-2, 5e-2]) # state covariance
+        self.Q = np.diag([1e-4, 1e-4, 1e-4, 2.4e-1, 1e-6]) # init process covariance values (2.4e-1 for velocity based on 3 x std dev of 0.16)
         self.R = np.diag([1e-2, 1e-2])  # init sensor covariance values
         self.last_time = None
 
@@ -180,11 +186,11 @@ def update_measurement(self, msg):
                 msg.pose.pose.orientation.z,
                 msg.twist.twist.linear.x,
                 0])
-            # extract 2x2 position covariance from the 6x6 pose covariance
-            self.R = np.reshape(np.stack((msg.pose.covariance[:2], msg.pose.covariance[6:8]), axis=0), (2, 2))
 
         # measurement vector
         y = [msg.pose.pose.position.x, msg.pose.pose.position.y]
+        # extract 2x2 position covariance from the 6x6 pose covariance
+        self.R = np.reshape(np.stack((msg.pose.covariance[:2], msg.pose.covariance[6:8]), axis=0), (2, 2))
         # perform measurement update
         self.x_hat, self.Sigma, self.debug = ukf_utils.ukf_update(self.x_hat, self.Sigma, y, self.R)
 
@@ -197,7 +203,10 @@ def loop(self):
 
         - Runs the predict step using the RK4-discretized dynamics.
         - Wraps heading and steering offset to keep them in valid ranges.
-        - Publishes steer offset, full state, and covariance at 100 Hz.
+        - Applies a low-pass filter to the steering offset.
+          - A low-pass filter filters out high-frequency noise in the estimate at the cost of responsiveness/increased lag;
+            this is okay since we expect the steering offset to vary slowly over time.
+        - Publishes steer offset (raw and filtered), full state, and covariance at 100 Hz.
         """
         if (not self.enabled) or (not self.start):
             return
@@ -208,10 +217,6 @@ def loop(self):
         self.x_hat[4] = self.wrap_angle(self.x_hat[4], np.pi/2)   # wrap steer offset
         self.last_time = time.time()
 
-        # wrap the steering offset to (-pi/2, pi/2]
-        steer_offset = np.rad2deg(self.wrap_angle(self.x_hat[4], np.pi/2))
-        self.offset_publisher.publish(Float64(data=steer_offset))
-
         state_msg = Float64MultiArray()
         state_msg.data = self.x_hat.tolist()
         state_msg.data[2] = np.rad2deg(state_msg.data[2]) # convert heading to deg
@@ -223,6 +228,20 @@ def loop(self):
         covar_msg.data = self.Sigma.flatten().tolist()
         self.state_covar_publisher.publish(covar_msg)
 
+        offset_variance = self.Sigma[-1, -1]
+
+        # Checks the offset variance is reasonable, corresponds to 6 deg std deviation.
+        if offset_variance < Constants.OFFSET_THRESHOLD:
+
+            # wrap the steering offset to (-pi/2, pi/2]
+            steer_offset = np.rad2deg(self.wrap_angle(self.x_hat[4], np.pi/2))
+            self.offset_publisher_raw.publish(Float64(data=steer_offset))
+
+            # apply low-pass filter to steering offset
+            steer_offset_filtered = self.lowPassFilter.update(steer_offset)
+            self.offset_publisher_filtered.publish(Float64(data=steer_offset_filtered))
+
+
 
 def main(args=None):
     rclpy.init(args=args)