Steering offset ukf #84

AnthonyHoneine · 2026-01-14T23:39:36Z

Overview

Added a steering offset to the SC UKF. Raw offset is passed into a low pass filter. Filtered offset is added to the command steering angle and passed to Firmware. Simulation allows testing the use, values and variance of the offset.

Linked Issues

#70

Tasks

Test during mock rolls
Initialization Convergence Check
Test during rolls

Testing

We will test the steering offset accuracy during mock rolls in FoxGlove, and then check the offset variance and UKF covariance to try to find a threshold to determine convergence.

…-Club/robobuggy-software into steering_offset_ukf

…nstructor

…lowup, added auton check to steering offset ukf, started publishing both corrected and raw steering_cmd in controller, corrected wheelbase constants

… update ros_to_bnyahaj.py accordingly

…offset is jumpy in sim

…fset_estimator, introduced steer offset raw and filtered topics, modified launch files to include steer offset configs

… covariance at each update

viv511 · 2026-01-27T23:30:17Z

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

+
+        # accuracy is the circular error radius (meters)
+        # k is the factor that maps between this radius and the Gaussian σ
+        k = 0.848867684498


Are the rest of the decimal points of this number fully accurate? does it matter much?

I could be wrong, but I looked up some stuff (and assuming this is like a CEP50 based conversation)

It might be worth writing the computation in a comment rather than just using a magic number?

is it the same as k = 1 / sqrt(2 * ln(2))?

https://www.wolframalpha.com/input?i=1+%2F+sqrt%282+*+ln%282%29%29

throw it in constants.py

rb_ws/src/buggy/scripts/simulator/engine.py

saransh323 · 2026-01-27T23:21:11Z

rb_ws/convert.yaml

@@ -1,4 +1,4 @@
 output_bags:
-  - uri: nand-raceline-mcap
+  - uri: rosbag2_2025_11_15-12_47_46_0


What is this?

Will revert, but basically a script to convert bags to .mcap (needs bags uri to work) @hpuranik

saransh323 · 2026-01-27T23:41:24Z

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

remove dead code

saransh323 · 2026-01-27T23:47:24Z

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

+        offset_variance = self.Sigma[-1, -1]
+
+        # Checks the offset variance is reasonable, corresponds to 6 deg std deviation.
+        if offset_variance < 0.1:


Can we make sure we remove magic numbers from our code? It helps with readability and finding bugs.

During our review, we found that this number is actually wrong.

6 degrees standard deviation corresponds to 0.10472 radians, but because this is standard deviation—we need to square the resulting value to find the variance.

The correct constant should be closer to 0.010966

In general:

Try not to use magic numbers

If we HAVE to use magic numbers, we should set a standard that we have a comment explaining the formula and the numbers we used to avoid confusion

*similar logic to the k = 0.8 yada yada constant

Also consider moving constants like these into the constants python file.
-vivek

also, what are observed values currently?

Will do! Current values are rougly 0.05

saransh323 · 2026-01-27T23:49:49Z

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

-        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


what does offset covariance usually settle to / hover around? (I believe we only have sim data right now, but let's mock roll and observe and then finalize this init value)

In simulation it hovers around 0.01-0.05, i agree with waiting until mock roll.

the-beast-coder

Just some questions about the changes

the-beast-coder · 2026-01-27T23:44:13Z

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

-        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


Where do these values come from?

These values are based on the following calculation:

(Rough estimate abt std dev) -> 3x confidence interval -> Variance.

the-beast-coder · 2026-01-27T23:47:06Z

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

        self.auton_enabled_prev = auton

    @classmethod
    def wrap_angle(cls, angle, limit=np.pi):


what does this function do?

Given an angle, it turns it into [-pi-pi range] (in radians). This ensure's that it's always an accurate angle [-pi, pi]

the-beast-coder · 2026-01-27T23:50:16Z

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

+        offset_variance = self.Sigma[-1, -1]
+
+        # Checks the offset variance is reasonable, corresponds to 6 deg std deviation.
+        if offset_variance < 0.1:


What if offset variance is more than 0.1? Doesnt that represent some sort of error happening

If the offset variance is greater than 0.1, this means we are very unsure of the offset. If this is the case, we don't want to publish any values because the data is too noisy.

the-beast-coder · 2026-01-27T23:51:39Z

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

        # 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))


What changed that made us take this out of the if condition?

We noticed a bug in the original implementation, so that the self.R was only set once. Instead, since it's related to the measurement, it can update each time.

saransh323 · 2026-01-28T00:18:03Z

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

+
+        # accuracy is the circular error radius (meters)
+        # k is the factor that maps between this radius and the Gaussian σ
+        k = 0.848867684498


throw it in constants.py

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

mehulgoel873 · 2026-01-28T23:40:03Z

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

        self.auton_enabled_prev = auton

    @classmethod
    def wrap_angle(cls, angle, limit=np.pi):


Given an angle, it turns it into [-pi-pi range] (in radians). This ensure's that it's always an accurate angle [-pi, pi]

mehulgoel873 · 2026-01-28T23:44:25Z

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

        # 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))


We noticed a bug in the original implementation, so that the self.R was only set once. Instead, since it's related to the measurement, it can update each time.

mehulgoel873 · 2026-01-28T23:45:58Z

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

+        offset_variance = self.Sigma[-1, -1]
+
+        # Checks the offset variance is reasonable, corresponds to 6 deg std deviation.
+        if offset_variance < 0.1:


Will do! Current values are rougly 0.05

mehulgoel873 · 2026-01-28T23:49:37Z

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

-        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


These values are based on the following calculation:

(Rough estimate abt std dev) -> 3x confidence interval -> Variance.

mehulgoel873 · 2026-01-28T23:49:43Z

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

-        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


Remove comment

mehulgoel873 · 2026-01-28T23:51:26Z

rb_ws/convert.yaml

@@ -1,4 +1,4 @@
 output_bags:
-  - uri: nand-raceline-mcap
+  - uri: rosbag2_2025_11_15-12_47_46_0


Will revert, but basically a script to convert bags to .mcap (needs bags uri to work) @hpuranik

…-Club/robobuggy-software into steering_offset_ukf

rk012 and others added 30 commits September 30, 2025 16:54

PR template update (#56)

63777dd

Merge branch 'rolls' into steering_offset_ukf

1cf48bd

Added offset ukf

5d06ba8

added original steering offset changes

400e136

Added state offset

56c052b

Finished essay

77ae804

Added steering offset publisher and feedback into the controller class

7a09e14

added steering offset to simulator and adjustable value in config file

2bd0452

Merge branch 'steering_offset_ukf' of https://github.com/CMU-Robotics…

98e6ab8

…-Club/robobuggy-software into steering_offset_ukf

Added accurate uncertainty estimates

75d08a4

Fixed launch files to take account nodes

664eb99

made controller's wheelbase constants use util/constants.py

93963c0

add default case in wheelbase assignment logic in Controller class co…

cf80f86

…nstructor

fixed wheelbase constants

f5a0f24

fixed bugs in steering offset ukf, added offset clamping to prevent b…

da09ac7

…lowup, added auton check to steering offset ukf, started publishing both corrected and raw steering_cmd in controller, corrected wheelbase constants

refactor: rename use_auton_steer to auton_steer in SCDebugInfoMsg and…

9dd5571

… update ros_to_bnyahaj.py accordingly

add offset estimator node to NAND and SC main launch files

b21ac81

fixed impoprts and field names

0bc5a69

Fixed steering_raw to not be a copy of steering

b7a3086

Started publishing true offset

5d6f770

Added stanley controller

045a484

Added offset ukf

d73087f

added original steering offset changes

004091b

Added state offset

7e47ee8

Finished essay

8a45116

Added steering offset publisher and feedback into the controller class

8236147

added steering offset to simulator and adjustable value in config file

37be135

Added accurate uncertainty estimates

a7e859a

Fixed launch files to take account nodes

7c04553

made controller's wheelbase constants use util/constants.py

9af29f5

mehulgoel873 and others added 9 commits November 19, 2025 17:14

Added process noise

c5d7a97

first draft at offset low pass filter

63674fc

draft 2 for LPF - fixed bug in class, import, minor debug fixes, but …

d56a250

…offset is jumpy in sim

refactoring some variables and constants

a4acaab

moved steer offset's low pass filter from controller_node to steer_of…

734a362

…fset_estimator, introduced steer offset raw and filtered topics, modified launch files to include steer offset configs

added a comment for low pass filter tradeoff

baf51bc

refactor steer offset ukf's measurement update to update/extract pose…

ad8ea09

… covariance at each update

Included Offset Var

f719918

added clarification comment to noise config

da3b50f

rk012 linked an issue Jan 14, 2026 that may be closed by this pull request

Short Circuit UKF for steering offset #70

Open

mehulgoel873 added 3 commits January 14, 2026 19:04

added convergence check

4834364

Merge branch 'rolls' into steering_offset_ukf

0e7693d

Fixed covariance init

d07c5cb

mehulgoel873 assigned mehulgoel873 and AnthonyHoneine Jan 20, 2026

mehulgoel873 requested review from rk012 and saransh323 January 20, 2026 22:57

mehulgoel873 marked this pull request as ready for review January 20, 2026 22:57

viv511 reviewed Jan 27, 2026

View reviewed changes

rb_ws/src/buggy/scripts/simulator/engine.py Show resolved Hide resolved

saransh323 requested changes Jan 27, 2026

View reviewed changes

the-beast-coder reviewed Jan 27, 2026

View reviewed changes

This comment was marked as resolved.

Sign in to view

saransh323 requested changes Jan 28, 2026

View reviewed changes

mehulgoel873 reviewed Jan 28, 2026

View reviewed changes

mehulgoel873 and others added 4 commits January 28, 2026 19:06

Added gitignore

7a5e1d2

Moved constants and made the calculations explicit

21b5627

Removed dead code and reverted file name

4ef114f

Merge branch 'steering_offset_ukf' of https://github.com/CMU-Robotics…

59c1816

…-Club/robobuggy-software into steering_offset_ukf

mehulgoel873 requested a review from saransh323 January 29, 2026 00:09

Steering offset ukf #84

Are you sure you want to change the base?

Steering offset ukf #84

Uh oh!

Conversation

AnthonyHoneine commented Jan 14, 2026 • edited by rk012 Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Overview

Linked Issues

Tasks

Testing

Uh oh!

viv511 Jan 27, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

mehulgoel873 Jan 28, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

saransh323 Jan 27, 2026 • edited by viv511 Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

the-beast-coder left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

This comment was marked as resolved.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

mehulgoel873 Jan 28, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Reviewers

AnthonyHoneine commented Jan 14, 2026 •

edited by rk012

Loading

viv511 Jan 27, 2026 •

edited

Loading

mehulgoel873 Jan 28, 2026 •

edited

Loading

saransh323 Jan 27, 2026 •

edited by viv511

Loading

mehulgoel873 Jan 28, 2026 •

edited

Loading