fix 3D wheel Q matrix: assign noise to correct rotation/translation axes by woosik2510 · Pull Request #85 · rpng/MINS

woosik2510 · 2026-07-31T05:28:28Z

Fixes two noise covariance bugs in preintegration_3D for the 3D wheel types.

Bug 1 - wrong noise index assignment (all three 3D types)

The 6-DOF preintegration state is [delta_R (roll, pitch, yaw), delta_p (x, y, z)]. For a differential-drive wheel on a flat surface, yaw (index 2) and forward x (index 3) are the measured DOFs; roll, pitch, lateral, and vertical are planar constraints.

The original Q matrix had the measured noise and planar constraint noise swapped on the rotation axes:

Index	DOF	Before	After
Q[0,0]	roll	`noise_w` / `noise_v/b`	`noise_p`
Q[2,2]	yaw	`noise_p`	`noise_w` / `noise_v/b`
Q[3,3] (Ang only)	forward x	`noise_w`	`noise_v`

This is invisible in simulation where noise_w == noise_v == noise_p, but causes filter inconsistency on real robots where the planar constraint noise is tuned much tighter than velocity noise (noise_p << noise_w): yaw uncertainty is underestimated, which propagates into position and causes high position NEES.

Bug 2 - Wheel3DLin variance missing factor of 2

For Wheel3DLin, raw measurements are left/right wheel linear speeds (vl, vr), each with noise noise_v. The derived quantities are omega = (vr-vl)/b and vx = (vr+vl)/2. Because vl and vr are independent, the correct variance is the sum of two contributions:

DOF	Before	After
omega (index 2)	`noise_v^2/b^2`	`2*noise_v^2/b^2`
vx (index 3)	`noise_v^2/4`	`noise_v^2/2`

The /2/2 on the vx line was computing (noise_v/2)^2 instead of Var((vr+vl)/2) = noise_v^2/2.

Closes #59.

github-actions · 2026-07-31T05:38:28Z

Unit Test Results

Test	Result	Duration
`test_UpdaterVicon`	✅ Passed	0.06s
`test_UpdaterWheel2D`	✅ Passed	0.06s
`test_UpdaterWheel3D`	✅ Passed	0.06s
`test_UpdaterWheelDt`	✅ Passed	0.06s
`test_UpdaterWheelInt`	✅ Passed	0.06s

5/5 passed

…independent noise sources

… axis assignments

github-actions · 2026-07-31T06:00:46Z

Simulation regression report - master vs PR

8 seeds/config, sim UD_Warehouse, informational only (does not block merge).
Delta is PR relative to master (mean). Lower is better for RMSE / Time; NEES closer to ~1-3 is well-calibrated.

full

Metric	master	PR	Delta (mean)
RMSE ori (deg)	0.164 +/- 0.038	0.164 +/- 0.038	+0.0%
RMSE pos (m)	0.053 +/- 0.009	0.053 +/- 0.009	+0.0%
NEES ori	3.400 +/- 1.300	3.400 +/- 1.300	+0.0%
NEES pos	2.700 +/- 0.700	2.700 +/- 0.700	+0.0%
Time (s)	31.300 +/- 0.300	31.300 +/- 0.300	+0.0%

stereo_gps

Metric	master	PR	Delta (mean)
RMSE ori (deg)	0.158 +/- 0.033	0.158 +/- 0.033	+0.0%
RMSE pos (m)	0.076 +/- 0.012	0.076 +/- 0.012	+0.0%
NEES ori	4.400 +/- 2.100	4.400 +/- 2.100	+0.0%
NEES pos	3.800 +/- 1.200	3.800 +/- 1.200	+0.0%
Time (s)	18.000 +/- 0.100	18.000 +/- 0.100	+0.0%

vicon

Metric	master	PR	Delta (mean)
RMSE ori (deg)	0.148 +/- 0.005	0.148 +/- 0.005	+0.0%
RMSE pos (m)	0.008 +/- 0.000	0.008 +/- 0.000	+0.0%
NEES ori	4.200 +/- 0.200	4.200 +/- 0.200	+0.0%
NEES pos	4.200 +/- 0.200	4.200 +/- 0.200	+0.0%
Time (s)	0.700 +/- 0.000	0.700 +/- 0.000	+0.0%

vicon_lidar

Metric	master	PR	Delta (mean)
RMSE ori (deg)	0.132 +/- 0.005	0.132 +/- 0.005	+0.0%
RMSE pos (m)	0.008 +/- 0.000	0.008 +/- 0.000	+0.0%
NEES ori	3.900 +/- 0.300	3.900 +/- 0.300	+0.0%
NEES pos	4.100 +/- 0.200	4.100 +/- 0.200	+0.0%
Time (s)	8.700 +/- 0.000	8.700 +/- 0.000	+0.0%

vio_stereo

Metric	master	PR	Delta (mean)
RMSE ori (deg)	0.174 +/- 0.036	0.174 +/- 0.036	+0.0%
RMSE pos (m)	0.111 +/- 0.032	0.111 +/- 0.032	+0.0%
NEES ori	2.000 +/- 0.900	2.000 +/- 0.900	+0.0%
NEES pos	12.600 +/- 16.500	12.600 +/- 16.500	+0.0%
Time (s)	18.000 +/- 0.100	17.900 +/- 0.100	-0.6%

wheel_2d_ang

Metric	master	PR	Delta (mean)
RMSE ori (deg)	0.129 +/- 0.005	0.129 +/- 0.005	+0.0%
RMSE pos (m)	0.007 +/- 0.000	0.007 +/- 0.000	+0.0%
NEES ori	3.900 +/- 0.200	3.900 +/- 0.200	+0.0%
NEES pos	4.100 +/- 0.300	4.100 +/- 0.300	+0.0%
Time (s)	0.700 +/- 0.000	0.700 +/- 0.000	+0.0%

wheel_2d_cen

Metric	master	PR	Delta (mean)
RMSE ori (deg)	0.134 +/- 0.005	0.134 +/- 0.005	+0.0%
RMSE pos (m)	0.007 +/- 0.000	0.007 +/- 0.000	+0.0%
NEES ori	4.500 +/- 0.500	4.500 +/- 0.500	+0.0%
NEES pos	4.500 +/- 0.300	4.500 +/- 0.300	+0.0%
Time (s)	0.500 +/- 0.000	0.500 +/- 0.000	+0.0%

wheel_2d_lin

Metric	master	PR	Delta (mean)
RMSE ori (deg)	0.133 +/- 0.005	0.133 +/- 0.005	+0.0%
RMSE pos (m)	0.007 +/- 0.000	0.007 +/- 0.000	+0.0%
NEES ori	4.300 +/- 0.400	4.300 +/- 0.400	+0.0%
NEES pos	4.300 +/- 0.300	4.300 +/- 0.300	+0.0%
Time (s)	0.700 +/- 0.000	0.700 +/- 0.000	+0.0%

wheel_3d_ang

Metric	master	PR	Delta (mean)
RMSE ori (deg)	0.125 +/- 0.005	0.113 +/- 0.004	-9.6%
RMSE pos (m)	0.007 +/- 0.000	0.006 +/- 0.000	-14.3%
NEES ori	4.000 +/- 0.200	3.700 +/- 0.200	-7.5%
NEES pos	3.700 +/- 0.200	3.700 +/- 0.200	+0.0%
Time (s)	0.700 +/- 0.000	0.700 +/- 0.000	+0.0%

wheel_3d_cen

Metric	master	PR	Delta (mean)
RMSE ori (deg)	0.132 +/- 0.005	0.115 +/- 0.004	-12.9%
RMSE pos (m)	0.007 +/- 0.000	0.006 +/- 0.000	-14.3%
NEES ori	4.000 +/- 0.200	3.900 +/- 0.200	-2.5%
NEES pos	3.800 +/- 0.200	3.900 +/- 0.200	+2.6%
Time (s)	0.400 +/- 0.000	0.500 +/- 0.000	+25.0%

wheel_3d_lin

Metric	master	PR	Delta (mean)
RMSE ori (deg)	0.129 +/- 0.006	0.114 +/- 0.004	-11.6%
RMSE pos (m)	0.007 +/- 0.000	0.006 +/- 0.000	-14.3%
NEES ori	3.900 +/- 0.300	3.800 +/- 0.200	-2.6%
NEES pos	3.800 +/- 0.200	3.800 +/- 0.200	+0.0%
Time (s)	0.700 +/- 0.000	0.800 +/- 0.000	+14.3%

…ng noise axis assignments" This reverts commit 603407b.

github-actions · 2026-07-31T06:02:08Z

Calibration Convergence Report

10 seeds · sim UD_Warehouse · informational only (does not block merge).

Scenario	Parameters	±3σ
Camera Time Offset	1/1 params	✅
Camera Extrinsic (IMU↔Cam)	6/6 params	✅
Camera Intrinsics	8/8 params	✅
GPS Time Offset	1/1 params	✅
GPS Extrinsic (IMU↔GPS)	3/3 params	✅
LiDAR Time Offset	1/1 params	✅
LiDAR Extrinsic (IMU↔LiDAR)	6/6 params	✅
Vicon Time Offset	1/1 params	✅
Vicon Extrinsic (IMU↔Vicon)	6/6 params	✅
Wheel Time Offset	1/1 params	✅
Wheel Extrinsic (IMU↔Wheel)	6/6 params	✅
Wheel Intrinsics (Wheel2DAng)	3/3 params	✅
Wheel Intrinsics (Wheel3DAng)	3/3 params	✅

Per-parameter details

✅ Camera Time Offset

| Parameter | final |error| | final σ | ±3σ |
|---|---|---|---|
| time offset (s) | 0.000179 | 0.00015 | ✅ |

✅ Camera Extrinsic (IMU↔Cam)

| Parameter | final |error| | final σ | ±3σ |
|---|---|---|---|
| φ_x (rad) | 0.00015 | 0.000271 | ✅ |
| φ_y (rad) | 6.79e-05 | 0.000287 | ✅ |
| φ_z (rad) | 0.000162 | 0.000268 | ✅ |
| p_x (m) | 0.00314 | 0.00382 | ✅ |
| p_y (m) | 0.00489 | 0.0049 | ✅ |
| p_z (m) | 0.0039 | 0.00348 | ✅ |

✅ Camera Intrinsics

| Parameter | final |error| | final σ | ±3σ |
|---|---|---|---|
| fu (px) | 0.109 | 0.0788 | ✅ |
| fv (px) | 0.115 | 0.0998 | ✅ |
| cu (px) | 0.085 | 0.0998 | ✅ |
| cv (px) | 0.0855 | 0.102 | ✅ |
| k₁ | 0.000165 | 0.000196 | ✅ |
| k₂ | 0.000104 | 0.000131 | ✅ |
| p₁ | 2.43e-05 | 3.69e-05 | ✅ |
| p₂ | 3.16e-05 | 2.35e-05 | ✅ |

✅ GPS Time Offset

| Parameter | final |error| | final σ | ±3σ |
|---|---|---|---|
| time offset (s) | 0.0031 | 0.00235 | ✅ |

✅ GPS Extrinsic (IMU↔GPS)

| Parameter | final |error| | final σ | ±3σ |
|---|---|---|---|
| p_x (m) | 0.0132 | 0.0229 | ✅ |
| p_y (m) | 0.0216 | 0.0233 | ✅ |
| p_z (m) | 0.0238 | 0.0237 | ✅ |

✅ LiDAR Time Offset

| Parameter | final |error| | final σ | ±3σ |
|---|---|---|---|
| time offset (s) | 0.00083 | 0.000187 | ✅ |

✅ LiDAR Extrinsic (IMU↔LiDAR)

| Parameter | final |error| | final σ | ±3σ |
|---|---|---|---|
| φ_x (rad) | 0.000537 | 0.000328 | ✅ |
| φ_y (rad) | 0.00029 | 0.000315 | ✅ |
| φ_z (rad) | 0.000253 | 0.000374 | ✅ |
| p_x (m) | 0.00329 | 0.00556 | ✅ |
| p_y (m) | 0.00573 | 0.00585 | ✅ |
| p_z (m) | 0.00659 | 0.00836 | ✅ |

✅ Vicon Time Offset

| Parameter | final |error| | final σ | ±3σ |
|---|---|---|---|
| time offset (s) | 0.000105 | 0.000148 | ✅ |

✅ Vicon Extrinsic (IMU↔Vicon)

| Parameter | final |error| | final σ | ±3σ |
|---|---|---|---|
| φ_x (rad) | 0.000155 | 0.000546 | ✅ |
| φ_y (rad) | 0.000295 | 0.000535 | ✅ |
| φ_z (rad) | 0.000384 | 0.000576 | ✅ |
| p_x (m) | 0.00313 | 0.00908 | ✅ |
| p_y (m) | 0.00442 | 0.00919 | ✅ |
| p_z (m) | 0.00327 | 0.00981 | ✅ |

✅ Wheel Time Offset

| Parameter | final |error| | final σ | ±3σ |
|---|---|---|---|
| time offset (s) | 8.74e-05 | 0.000212 | ✅ |

✅ Wheel Extrinsic (IMU↔Wheel)

| Parameter | final |error| | final σ | ±3σ |
|---|---|---|---|
| φ_x (rad) | 0.00413 | 0.00207 | ✅ |
| φ_y (rad) | 0.000256 | 0.000346 | ✅ |
| φ_z (rad) | 0.000184 | 0.00036 | ✅ |
| p_x (m) | 0.000521 | 0.0023 | ✅ |
| p_y (m) | 0.000627 | 0.00216 | ✅ |
| p_z (m) | 0.0204 | 0.0237 | ✅ |

✅ Wheel Intrinsics (Wheel2DAng)

| Parameter | final |error| | final σ | ±3σ |
|---|---|---|---|
| left radius (m) | 1.62e-05 | 2.6e-05 | ✅ |
| right radius (m) | 1.87e-05 | 2.6e-05 | ✅ |
| baseline (m) | 0.000663 | 0.000941 | ✅ |

✅ Wheel Intrinsics (Wheel3DAng)

| Parameter | final |error| | final σ | ±3σ |
|---|---|---|---|
| left radius (m) | 1.51e-05 | 8.43e-05 | ✅ |
| right radius (m) | 1.29e-05 | 8.5e-05 | ✅ |
| baseline (m) | 0.00072 | 0.00209 | ✅ |

… values

This reverts commit ec7efe8.

…regression" This reverts commit ee25e71.

… to defaults

fix 3D wheel Q matrix: assign noise to correct rotation/translation axes

da219f4

woosik2510 requested a review from WoosikLee2510 as a code owner July 31, 2026 05:28

woosik2510 added 2 commits July 30, 2026 22:46

fix Wheel3DLin Q: omega and vx variance missing factor of 2 from two …

841b155

…independent noise sources

extract ComputeNoiseQ3D static method, add unit tests verifying noise…

603407b

… axis assignments

Revert "extract ComputeNoiseQ3D static method, add unit tests verifyi…

bd78bc8

…ng noise axis assignments" This reverts commit 603407b.

woosik2510 added 2 commits July 30, 2026 23:23

sim: use distinct wheel noise values to expose Q axis bug in regression

ee25e71

ci: expand wheel eval to all 9 type combos with vicon, distinct noise…

013d1ba

… values

woosik2510 mentioned this pull request Jul 31, 2026

ci: expand wheel regression to all 9 type combinations #86

Closed

woosik2510 added 7 commits July 30, 2026 23:28

ci: replace stereo_lidar/stereo_gps with vicon_lidar/vicon_gps

b8ee399

ci: use imu+gps instead of vicon_gps

b34b4a5

ci: full scenario uses vicon instead of stereo camera

ec7efe8

Revert "ci: full scenario uses vicon instead of stereo camera"

0797023

This reverts commit ec7efe8.

ci: restore stereo_gps, remove non-planar 3D wheel scenarios

8db7dd0

Revert "sim: use distinct wheel noise values to expose Q axis bug in …

f7cd5a3

…regression" This reverts commit ee25e71.

ci: inject distinct wheel noise via env var, revert config_wheel.yaml…

d3ace63

… to defaults

WoosikLee2510 merged commit adafa57 into master Jul 31, 2026
39 checks passed

WoosikLee2510 deleted the fix/wheel-3d-q-matrix branch July 31, 2026 11:26

woosik2510 mentioned this pull request Jul 31, 2026

Question about the wheel odometry noise definitions #59

Closed

Uh oh!

Conversation

woosik2510 commented Jul 31, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Bug 1 - wrong noise index assignment (all three 3D types)

Bug 2 - Wheel3DLin variance missing factor of 2

Uh oh!

github-actions Bot commented Jul 31, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Unit Test Results

Uh oh!

github-actions Bot commented Jul 31, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Simulation regression report - master vs PR

Uh oh!

github-actions Bot commented Jul 31, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Calibration Convergence Report

✅ Camera Time Offset

| Parameter | final |error| | final σ | ±3σ | |---|---|---|---| | time offset (s) | 0.000179 | 0.00015 | ✅ |

✅ Camera Extrinsic (IMU↔Cam)

✅ Camera Intrinsics

✅ GPS Time Offset

| Parameter | final |error| | final σ | ±3σ | |---|---|---|---| | time offset (s) | 0.0031 | 0.00235 | ✅ |

✅ GPS Extrinsic (IMU↔GPS)

| Parameter | final |error| | final σ | ±3σ | |---|---|---|---| | p_x (m) | 0.0132 | 0.0229 | ✅ | | p_y (m) | 0.0216 | 0.0233 | ✅ | | p_z (m) | 0.0238 | 0.0237 | ✅ |

✅ LiDAR Time Offset

| Parameter | final |error| | final σ | ±3σ | |---|---|---|---| | time offset (s) | 0.00083 | 0.000187 | ✅ |

✅ LiDAR Extrinsic (IMU↔LiDAR)

✅ Vicon Time Offset

| Parameter | final |error| | final σ | ±3σ | |---|---|---|---| | time offset (s) | 0.000105 | 0.000148 | ✅ |

✅ Vicon Extrinsic (IMU↔Vicon)

✅ Wheel Time Offset

| Parameter | final |error| | final σ | ±3σ | |---|---|---|---| | time offset (s) | 8.74e-05 | 0.000212 | ✅ |

✅ Wheel Extrinsic (IMU↔Wheel)

✅ Wheel Intrinsics (Wheel2DAng)

| Parameter | final |error| | final σ | ±3σ | |---|---|---|---| | left radius (m) | 1.62e-05 | 2.6e-05 | ✅ | | right radius (m) | 1.87e-05 | 2.6e-05 | ✅ | | baseline (m) | 0.000663 | 0.000941 | ✅ |

✅ Wheel Intrinsics (Wheel3DAng)

| Parameter | final |error| | final σ | ±3σ | |---|---|---|---| | left radius (m) | 1.51e-05 | 8.43e-05 | ✅ | | right radius (m) | 1.29e-05 | 8.5e-05 | ✅ | | baseline (m) | 0.00072 | 0.00209 | ✅ |

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

woosik2510 commented Jul 31, 2026 •

edited

Loading

github-actions Bot commented Jul 31, 2026 •

edited

Loading

github-actions Bot commented Jul 31, 2026 •

edited

Loading

github-actions Bot commented Jul 31, 2026 •

edited

Loading

| Parameter | final |error| | final σ | ±3σ |
|---|---|---|---|
| time offset (s) | 0.000179 | 0.00015 | ✅ |

| Parameter | final |error| | final σ | ±3σ |
|---|---|---|---|
| time offset (s) | 0.0031 | 0.00235 | ✅ |

| Parameter | final |error| | final σ | ±3σ |
|---|---|---|---|
| p_x (m) | 0.0132 | 0.0229 | ✅ |
| p_y (m) | 0.0216 | 0.0233 | ✅ |
| p_z (m) | 0.0238 | 0.0237 | ✅ |

| Parameter | final |error| | final σ | ±3σ |
|---|---|---|---|
| time offset (s) | 0.00083 | 0.000187 | ✅ |

| Parameter | final |error| | final σ | ±3σ |
|---|---|---|---|
| time offset (s) | 0.000105 | 0.000148 | ✅ |

| Parameter | final |error| | final σ | ±3σ |
|---|---|---|---|
| time offset (s) | 8.74e-05 | 0.000212 | ✅ |

| Parameter | final |error| | final σ | ±3σ |
|---|---|---|---|
| left radius (m) | 1.62e-05 | 2.6e-05 | ✅ |
| right radius (m) | 1.87e-05 | 2.6e-05 | ✅ |
| baseline (m) | 0.000663 | 0.000941 | ✅ |

| Parameter | final |error| | final σ | ±3σ |
|---|---|---|---|
| left radius (m) | 1.51e-05 | 8.43e-05 | ✅ |
| right radius (m) | 1.29e-05 | 8.5e-05 | ✅ |
| baseline (m) | 0.00072 | 0.00209 | ✅ |