From 7d3731b40c7812588a7feca637173f070aacab8f Mon Sep 17 00:00:00 2001 From: Elon-Jiang <191222665+Elon-Jiang@users.noreply.github.com> Date: Tue, 10 Nov 2026 19:04:59 +0800 Subject: [PATCH] Preserve clean controls in histogram localization observations Co-authored-by: OpenAI Codex --- .../histogram_filter/histogram_filter.py | 2 +- .../histogram_filter_localization_main.rst | 5 +++ tests/test_histogram_filter.py | 34 +++++++++++++++++++ 3 files changed, 40 insertions(+), 1 deletion(-) diff --git a/Localization/histogram_filter/histogram_filter.py b/Localization/histogram_filter/histogram_filter.py index 17cfc2e14c..b3e21b0ff1 100644 --- a/Localization/histogram_filter/histogram_filter.py +++ b/Localization/histogram_filter/histogram_filter.py @@ -136,7 +136,7 @@ def observation(xTrue, u, RFID): z = np.vstack((z, zi)) # add noise to speed - ud = u[:, :] + ud = u.copy() ud[0] += np.random.randn() * NOISE_SPEED return xTrue, z, ud diff --git a/docs/modules/2_localization/histogram_filter_localization/histogram_filter_localization_main.rst b/docs/modules/2_localization/histogram_filter_localization/histogram_filter_localization_main.rst index b5697d2dd9..ae1ad93c74 100644 --- a/docs/modules/2_localization/histogram_filter_localization/histogram_filter_localization_main.rst +++ b/docs/modules/2_localization/histogram_filter_localization/histogram_filter_localization_main.rst @@ -14,6 +14,11 @@ but x,y positions are unknown. The filter uses speed input and range observations from RFID for localization. +The simulator propagates the true position with the clean control input and +returns a separate noisy speed measurement to the filter. Sensor noise does +not modify the caller's control input or accumulate in the true trajectory +when a command is reused. + Initial position information is not needed. Code Link diff --git a/tests/test_histogram_filter.py b/tests/test_histogram_filter.py index 4474ead097..e5f1151b97 100644 --- a/tests/test_histogram_filter.py +++ b/tests/test_histogram_filter.py @@ -1,4 +1,6 @@ import conftest +import numpy as np +import pytest from Localization.histogram_filter import histogram_filter as m @@ -8,5 +10,37 @@ def test1(): m.main() +@pytest.mark.parametrize("readonly", [False, True]) +def test_observation_preserves_clean_control(readonly): + control = m.calc_control_input() + original = control.copy() + control.flags.writeable = not readonly + state = np.zeros((4, 1)) + landmarks = np.array([[100.0, 100.0]]) + + next_state, ranges, measured_control = m.observation(state, control, landmarks) + + np.testing.assert_array_equal(control, original) + np.testing.assert_array_equal(next_state, m.motion_model(state, original)) + assert ranges.shape == (0, 3) + assert not np.shares_memory(measured_control, control) + measured_control[:] = 0.0 + np.testing.assert_array_equal(control, original) + + +def test_reused_control_keeps_true_trajectory_independent_of_sensor_noise(): + control = m.calc_control_input() + original = control.copy() + state = np.zeros((4, 1)) + expected = state.copy() + landmarks = np.array([[100.0, 100.0]]) + + for _ in range(3): + state, _, _ = m.observation(state, control, landmarks) + expected = m.motion_model(expected, original) + np.testing.assert_array_equal(control, original) + np.testing.assert_allclose(state, expected) + + if __name__ == '__main__': conftest.run_this_test(__file__)