From 94df091d3fba8b4e9ea706ec7d9144c89d1bd0a3 Mon Sep 17 00:00:00 2001 From: ThreeMonth03 Date: Sat, 16 May 2026 21:06:38 +0800 Subject: [PATCH] Add radar example to test Kalman filter --- cpp/modmesh/linalg/kalman_filter.hpp | 43 +++++++++++++++ .../linalg/pymod/wrap_kalman_filter.cpp | 38 +++++++++++++ tests/test_linalg.py | 55 +++++++++++++++++++ 3 files changed, 136 insertions(+) diff --git a/cpp/modmesh/linalg/kalman_filter.hpp b/cpp/modmesh/linalg/kalman_filter.hpp index 2794a4e36..fff187d6a 100644 --- a/cpp/modmesh/linalg/kalman_filter.hpp +++ b/cpp/modmesh/linalg/kalman_filter.hpp @@ -155,6 +155,49 @@ class KalmanFilter check_dimensions(); } + /** + * @brief Construct a Kalman filter with explicit covariance matrices. + * + * @details + * Creates a Kalman filter with the specified system matrices and covariance + * matrices. This overload is useful for examples or physical models whose + * process noise, measurement noise, or initial state uncertainty are not + * scaled identity matrices. + * + * @param x Initial state vector. + * @param f State transition matrix F. + * @param b Control matrix B (empty to disable control input u). + * @param h Measurement matrix H. + * @param q Process noise covariance Q. + * @param r Measurement noise covariance R. + * @param p Initial state covariance P. + * @param jitter Numerical stability jitter. + */ + KalmanFilter( + array_type const & x, // FIXME: NOLINT(modernize-pass-by-value) + array_type const & f, // FIXME: NOLINT(modernize-pass-by-value) + array_type const & b, // FIXME: NOLINT(modernize-pass-by-value) + array_type const & h, // FIXME: NOLINT(modernize-pass-by-value) + array_type const & q, // FIXME: NOLINT(modernize-pass-by-value) + array_type const & r, // FIXME: NOLINT(modernize-pass-by-value) + array_type const & p, // FIXME: NOLINT(modernize-pass-by-value) + real_type jitter) + : m_state_size(x.shape(0)) + , m_measurement_size(h.shape(0)) + , m_control_size((b.ndim() == 2) ? b.shape(1) : 0) + , m_f(f) + , m_q(q) + , m_h(h) + , m_r(r) + , m_p(p) + , m_b(b) + , m_x(x) + , m_i(array_type::eye(m_state_size)) + , m_jitter(jitter) + { + check_dimensions(); + } + array_type const & state() const { return m_x; } array_type const & covariance() const { return m_p; } diff --git a/cpp/modmesh/linalg/pymod/wrap_kalman_filter.cpp b/cpp/modmesh/linalg/pymod/wrap_kalman_filter.cpp index a89446adf..07e057d38 100644 --- a/cpp/modmesh/linalg/pymod/wrap_kalman_filter.cpp +++ b/cpp/modmesh/linalg/pymod/wrap_kalman_filter.cpp @@ -111,6 +111,44 @@ class MODMESH_PYTHON_WRAPPER_VISIBILITY WrapKalmanFilter py::arg("process_noise"), py::arg("measurement_noise"), py::arg("jitter") = static_cast(1e-9)) + .def( + py::init( + [](array_type const & x, + array_type const & f, + py::object const & b, + array_type const & h, + array_type const & process_noise_covariance, + array_type const & measurement_noise_covariance, + array_type const & covariance, + real_type jitter) + { + array_type b_array; + if (b.is_none()) + { + b_array = array_type(small_vector{x.shape(0), 0}); + } + else + { + b_array = b.cast(); + } + return wrapped_type( + x, + f, + b_array, + h, + process_noise_covariance, + measurement_noise_covariance, + covariance, + jitter); + }), + py::arg("x"), + py::arg("f"), + py::arg("b") = py::none(), + py::arg("h"), + py::arg("q"), + py::arg("r"), + py::arg("p"), + py::arg("jitter") = static_cast(1e-9)) .def_property_readonly( "state", &wrapped_type::state) diff --git a/tests/test_linalg.py b/tests/test_linalg.py index 892c6b15a..94e8d9ca6 100644 --- a/tests/test_linalg.py +++ b/tests/test_linalg.py @@ -406,6 +406,61 @@ def sa_from_np(arr: np.ndarray, cls): raise ValueError("sa_from_np supports only 1D or 2D arrays") +class KalmanFilterRadarExampleTC(unittest.TestCase): + + def test_kalmanfilter_net_radar_example(self): + # Reference: https://kalmanfilter.net/ + dt = 5.0 + x0 = np.array([10000.0, 200.0]) + f = np.array([[1.0, dt], + [0.0, 1.0]]) + h = np.eye(2) + p0 = np.array([[16.0, 0.0], + [0.0, 0.25]]) + q = np.array([[6.25, 2.5], + [2.5, 1.0]]) + r = np.array([[36.0, 0.0], + [0.0, 2.25]]) + z1 = np.array([11020.0, 202.0]) + + kf = mm.KalmanFilterFp64( + x=sa_from_np(x0, mm.SimpleArrayFloat64), + f=sa_from_np(f, mm.SimpleArrayFloat64), + h=sa_from_np(h, mm.SimpleArrayFloat64), + q=sa_from_np(q, mm.SimpleArrayFloat64), + r=sa_from_np(r, mm.SimpleArrayFloat64), + p=sa_from_np(p0, mm.SimpleArrayFloat64), + jitter=0.0, + ) + + kf.predict() + + x_pred_expected = np.array([11000.0, 200.0]) + p_pred_expected = np.array([[28.5, 3.75], + [3.75, 1.25]]) + + np.testing.assert_allclose( + kf.state.ndarray, x_pred_expected, atol=1e-12, rtol=0.0) + np.testing.assert_allclose( + kf.covariance.ndarray, p_pred_expected, atol=1e-12, rtol=0.0) + + kf.update(sa_from_np(z1, mm.SimpleArrayFloat64)) + + x_update_expected = np.array([ + 11009.371124889283, + 201.42604074402126, + ]) + p_update_expected = np.array([ + [14.572187776793623, 1.4348981399468559], + [1.4348981399468559, 0.7074844995571303], + ]) + + np.testing.assert_allclose( + kf.state.ndarray, x_update_expected, atol=1e-12, rtol=0.0) + np.testing.assert_allclose( + kf.covariance.ndarray, p_update_expected, atol=1e-12, rtol=0.0) + + class TestKnownIssues603(unittest.TestCase): @unittest.expectedFailure