From c905786831f497c31f608c95e1fb1c4c7553e73b Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Thu, 8 Jan 2026 16:46:41 +0100 Subject: [PATCH] user defined gate function setup --- .../vortex_filtering/filters/README.md | 2 +- .../vortex_filtering/filters/immipda.hpp | 5 +- .../include/vortex_filtering/filters/ipda.hpp | 77 ++++++++++-- .../include/vortex_filtering/filters/pdaf.hpp | 113 +++++++++++++++--- vortex-filtering/test/immipda_test.cpp | 35 +++--- vortex-filtering/test/ipda_test.cpp | 2 - vortex-filtering/test/pdaf_test.cpp | 9 +- 7 files changed, 187 insertions(+), 56 deletions(-) diff --git a/vortex-filtering/include/vortex_filtering/filters/README.md b/vortex-filtering/include/vortex_filtering/filters/README.md index e4417ec..157b0a7 100644 --- a/vortex-filtering/include/vortex_filtering/filters/README.md +++ b/vortex-filtering/include/vortex_filtering/filters/README.md @@ -273,7 +273,7 @@ The perceived measurements. This parameter consists of an `Eigen::Vector`. It sh This will be a short description of what is happening inside the step function. For a more clear and specific explanation of the steps of the PDAF please look into the recommended textbooks. * The step function will first use the dynamic model to calculate the next state (only considering the given model). Then the sensor model is used to convert the predicted state into the measurement space. That means to get the value, we would measure with our sensor if the perceived state is like the predicted state (of the dynamic model). Concerning our rocket example: We predict the drive temperature with the dynamic model and use the sensor model to convert the drive temperature to the temperature we would measure from the outside. Both of those steps are done in one line of code by using the EKF explained earlier. -* The second step is the gating of the measurements. This is to exclude measurements that are too far away. So, we can assume that they are definitely clutter, and we won't use them. The **mahalanobis_threshold** parameter is used to define the threshold used for gating. This value will scale the covariance of the predicted state. All measurements inside this will be considered. Additionally, **min_gate_threshold** and **max_gate_threshold** are used here. Since we scale the over-time-changing covariance, we implemented a min and max threshold. If the scaled covariance is too large or too small, we still take measurements in a fixed area into account. +* The second step is the gating of the measurements. This is to exclude measurements that are too far away. So, we can assume that they are definitely clutter, and we won't use them. The **mahalanobis_threshold** parameter is used to define the threshold used for gating. This value will scale the covariance of the predicted state. All measurements inside this will be considered. * The next step is the update states step. All measurements inside the gate will be compared to the predicted state estimate (by the dynamic model). This results in a Gaussian distribution for all of these measurements. * In the last step, the weighted average of estimated Gaussian distributions will be calculated. This weighted average will be the final output of the PDAF and is considered to be the current state estimate. Therefore, it will be the previous state estimate in the next iteration. diff --git a/vortex-filtering/include/vortex_filtering/filters/immipda.hpp b/vortex-filtering/include/vortex_filtering/filters/immipda.hpp index 43541a0..5ee9f81 100644 --- a/vortex-filtering/include/vortex_filtering/filters/immipda.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/immipda.hpp @@ -103,8 +103,9 @@ class IMMIPDA { // Gate measurements Arr_nXb gated_measurements(ImmModelT::N_MODELS, m_k); [&](std::index_sequence) { - ((gated_measurements.row(s_k) = PDAF_::apply_gate( - z_measurements, z_est_preds.at(s_k), {config.pdaf})), + ((gated_measurements.row(s_k) = + PDAF_::apply_gate(z_measurements, z_est_preds.at(s_k), + config.pdaf.mahalanobis_threshold)), ...); }(std::make_index_sequence{}); diff --git a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp index dd9d4d4..72582bf 100644 --- a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp @@ -32,6 +32,7 @@ class IPDA { using Gauss_x = typename T::Gauss_x; using Gauss_xX = std::vector; + using Vec_z = typename T::Vec_z; using Arr_zXd = Eigen::Array; using Arr_1Xb = Eigen::Array; using EKF = @@ -169,14 +170,18 @@ class IPDA { * @param z_measurements Array of measurements * @param existence_prob_pred The predicted existence probability * @param config Configuration for the IPDA + * @param gate_fn Function to apply the gate * @return `UpdateResult` */ + template + requires Gate static UpdateResult update(const SensModT& sens_mod, const Gauss_x& x_pred, const Gauss_z& z_pred, const Arr_zXd& z_measurements, double existence_prob_pred, - const Config& config) { + const Config& config, + GateFn&& gate_fn) { double clutter_intensity = config.pdaf.clutter_intensity; if (config.ipda.estimate_clutter) { @@ -190,7 +195,8 @@ class IPDA { auto [x_post, x_updates, gated_measurements, z_inside_meas, z_likelihoods] = - PDAF::update(sens_mod, x_pred, z_pred, z_measurements, pdaf_cfg); + PDAF::update(sens_mod, x_pred, z_pred, z_measurements, pdaf_cfg, + std::forward(gate_fn)); double existence_prob_upd = existence_prob_pred; if (z_measurements.cols() > 0 || @@ -199,12 +205,35 @@ class IPDA { z_likelihoods, existence_prob_pred, pdaf_cfg.pdaf); } - return UpdateResult{.x_post = x_post, - .x_updates = x_updates, - .gated_measurements = gated_measurements, - .z_inside_meas = z_inside_meas, - .existence_prob_upd = existence_prob_upd, - .clutter_intensity = clutter_intensity}; + return {.x_post = x_post, + .x_updates = x_updates, + .gated_measurements = gated_measurements, + .z_inside_meas = z_inside_meas, + .existence_prob_upd = existence_prob_upd, + .clutter_intensity = clutter_intensity}; + } + + /** + * @brief Performs one IPDA update step + * @param sens_mod The sensor model + * @param x_pred The predicted state + * @param z_pred The predicted measurement + * @param z_measurements Array of measurements + * @param existence_prob_pred The predicted existence probability + * @param config Configuration for the IPDA + * @param gate_fn Function to apply the gate + * @return `UpdateResult` + */ + static UpdateResult update(const SensModT& sens_mod, + const Gauss_x& x_pred, + const Gauss_z& z_pred, + const Arr_zXd& z_measurements, + double existence_prob_pred, + const Config& config) { + return update( + sens_mod, x_pred, z_pred, z_measurements, existence_prob_pred, + config, + typename PDAF::MahalanobisGate{config.pdaf.mahalanobis_threshold}); } /** @@ -217,22 +246,26 @@ class IPDA { * @param state_est_prev The previous estimated state * @param z_measurements Array of measurements * @param config Configuration for the IPDA + * @param gate_fn Function to apply the gate * @return `StepResult` The result of the IPDA step and some intermediate * results */ + template + requires Gate static StepResult step(const DynModT& dyn_mod, const SensModT& sens_mod, double dt, const State& state_est_prev, const Arr_zXd& z_measurements, - const Config& config) { + const Config& config, + GateFn&& gate_fn) { auto [x_pred, z_pred, existence_prob_pred] = predict(dyn_mod, sens_mod, dt, state_est_prev, config); auto [x_post, x_updates, gated_measurements, z_inside_meas, existence_prob_upd, clutter_intensity] = update(sens_mod, x_pred, z_pred, z_measurements, - existence_prob_pred, config); + existence_prob_pred, config, std::forward(gate_fn)); // clang-format off return { @@ -249,6 +282,30 @@ class IPDA { // clang-format on } + /** + * @brief Perform one step of the Integrated Probabilistic Data Association + * Filter + * + * @param dyn_mod The dynamic model + * @param sens_mod The sensor model + * @param dt Time step in seconds + * @param state_est_prev The previous estimated state + * @param z_measurements Array of measurements + * @param config Configuration for the IPDA + * @return `StepResult` The result of the IPDA step and some intermediate + * results + */ + static StepResult step(const DynModT& dyn_mod, + const SensModT& sens_mod, + double dt, + const State& state_est_prev, + const Arr_zXd& z_measurements, + const Config& config) { + return step( + dyn_mod, sens_mod, dt, state_est_prev, z_measurements, config, + typename PDAF::MahalanobisGate{config.pdaf.mahalanobis_threshold}); + } + /** * @brief Calculates the predicted existence probability * @param existence_prob_est (r_{k-1}) The previous existence probability. diff --git a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp index 86da919..6762c82 100644 --- a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include #include @@ -14,13 +15,17 @@ namespace vortex::filter { namespace config { struct PDAF { double mahalanobis_threshold = 1.0; - double min_gate_threshold = 0.0; - double max_gate_threshold = std::numeric_limits::max(); double prob_of_detection = 1.0; double clutter_intensity = 1.0; }; } // namespace config +/** + * @brief Concept for gating functions used in PDAF + */ +template +concept Gate = std::predicate; + template class PDAF { @@ -110,6 +115,19 @@ class PDAF { Arr_1Xb gated_measurements; }; + /** + * @brief Default gating function based on Mahalanobis distance + */ + struct MahalanobisGate { + double threshold; + + bool operator()(const Vec_z& z, const Gauss_z& z_pred) const { + double mahal = z_pred.mahalanobis_distance(z); + + return mahal <= threshold; + } + }; + PDAF() = delete; /** @@ -141,14 +159,19 @@ class PDAF { * @param z_pred The predicted measurement * @param z_measurements Array of measurements * @param config Configuration for the PDAF + * @param gate_fn Function to apply the gate * @return `UpdateResult` */ + template + requires Gate static UpdateResult update(const SensModT& sens_mod, const Gauss_x& x_pred, const Gauss_z& z_pred, const Arr_zXd& z_measurements, - const Config& config) { - auto gated_measurements = apply_gate(z_measurements, z_pred, config); + const Config& config, + GateFn&& gate_fn) { + auto gated_measurements = + apply_gate(z_measurements, z_pred, std::forward(gate_fn)); auto z_inside_meas = get_inside_measurements(z_measurements, gated_measurements); @@ -173,6 +196,25 @@ class PDAF { }; } + /** + * @brief Perform one PDAF update step + * + * @param sens_mod The sensor model + * @param x_pred The predicted state + * @param z_pred The predicted measurement + * @param z_measurements Array of measurements + * @param config Configuration for the PDAF + * @return `UpdateResult` + */ + static UpdateResult update(const SensModT& sens_mod, + const Gauss_x& x_pred, + const Gauss_z& z_pred, + const Arr_zXd& z_measurements, + const Config& config) { + return update(sens_mod, x_pred, z_pred, z_measurements, config, + MahalanobisGate{config.pdaf.mahalanobis_threshold}); + } + /** * @brief Perform one step of the Probabilistic Data Association Filter * @@ -182,20 +224,24 @@ class PDAF { * @param x_est The estimated state * @param z_measurements Array of measurements * @param config Configuration for the PDAF + * @param gate_fn Function to apply the gate * @return `StepResult` The result of the PDAF step and some intermediate * results */ + template + requires Gate static StepResult step(const DynModT& dyn_mod, const SensModT& sens_mod, double dt, const Gauss_x& x_est, const Arr_zXd& z_measurements, - const Config& config) { + const Config& config, + GateFn&& gate_fn) { auto [x_pred, z_pred] = predict(dyn_mod, sens_mod, dt, x_est); auto [x_post, x_updates, gated_measurements, z_inside_meas, - z_likelihoods] = - update(sens_mod, x_pred, z_pred, z_measurements, config); + z_likelihoods] = update(sens_mod, x_pred, z_pred, z_measurements, + config, std::forward(gate_fn)); return StepResult{ .x_post = x_post, .x_pred = x_pred, @@ -205,34 +251,63 @@ class PDAF { }; } + /** + * @brief Perform one step of the Probabilistic Data Association Filter + * + * @param dyn_mod The dynamic model + * @param sens_mod The sensor model + * @param dt Time step in seconds + * @param x_est The estimated state + * @param z_measurements Array of measurements + * @param config Configuration for the PDAF + * @return `StepResult` The result of the PDAF step and some intermediate + * results + */ + static StepResult step(const DynModT& dyn_mod, + const SensModT& sens_mod, + double dt, + const Gauss_x& x_est, + const Arr_zXd& z_measurements, + const Config& config) { + return step(dyn_mod, sens_mod, dt, x_est, z_measurements, config, + MahalanobisGate{config.pdaf.mahalanobis_threshold}); + } + /** * @brief Apply gate to the measurements * * @param z_measurements Array of measurements * @param z_pred Predicted measurement - * @param config Configuration for the PDAF + * @param gate_fn Function to apply the gate * @return `Arr_1Xb` Indices of the measurements that are inside the gate */ + template + requires Gate static Arr_1Xb apply_gate(const Arr_zXd& z_measurements, const Gauss_z& z_pred, - Config config) { - double mahalanobis_threshold = config.pdaf.mahalanobis_threshold; - double min_gate_threshold = config.pdaf.min_gate_threshold; - double max_gate_threshold = config.pdaf.max_gate_threshold; - + GateFn&& gate_fn) { Arr_1Xb gated_measurements(1, z_measurements.cols()); for (size_t a_k = 0; const Vec_z& z_k : z_measurements.colwise()) { - double mahalanobis_distance = z_pred.mahalanobis_distance(z_k); - double regular_distance = (z_pred.mean() - z_k).norm(); - gated_measurements(a_k++) = - (mahalanobis_distance <= mahalanobis_threshold || - regular_distance <= min_gate_threshold) && - regular_distance <= max_gate_threshold; + gated_measurements(a_k++) = gate_fn(z_k, z_pred); } return gated_measurements; } + /** + * @brief Apply gate to the measurements + * + * @param z_measurements Array of measurements + * @param z_pred Predicted measurement + * @param config Configuration for the PDAF + * @return `Arr_1Xb` Indices of the measurements that are inside the gate + */ + static Arr_1Xb apply_gate(const Arr_zXd& z_measurements, + const Gauss_z& z_pred, + double threshold) { + return apply_gate(z_measurements, z_pred, MahalanobisGate{threshold}); + } + /** * @brief Get the measurements that are inside the gate * diff --git a/vortex-filtering/test/immipda_test.cpp b/vortex-filtering/test/immipda_test.cpp index f919d7a..8952aa7 100644 --- a/vortex-filtering/test/immipda_test.cpp +++ b/vortex-filtering/test/immipda_test.cpp @@ -16,25 +16,22 @@ class IMMIPDA : public ::testing::Test { IMMIPDA() : imm_model_(jump_matrix, hold_times, DynMod1_(0.5), DynMod2_(0.5)), sensor_model_(2), - config_{ - .pdaf = - { - .mahalanobis_threshold = 1.0, - .min_gate_threshold = 0.0, - .max_gate_threshold = std::numeric_limits::max(), - .prob_of_detection = 0.9, - .clutter_intensity = 1.0, - }, - .ipda = - { - .prob_of_survival = 0.9, - .estimate_clutter = true, - .update_existence_probability_on_no_detection = true, - }, - .immipda = { - .states_min_max = {{S::position, {-100.0, 100.0}}, - {S::velocity, {-10.0, 10.0}}}, - }} {}; + config_{.pdaf = + { + .mahalanobis_threshold = 1.0, + .prob_of_detection = 0.9, + .clutter_intensity = 1.0, + }, + .ipda = + { + .prob_of_survival = 0.9, + .estimate_clutter = true, + .update_existence_probability_on_no_detection = true, + }, + .immipda = { + .states_min_max = {{S::position, {-100.0, 100.0}}, + {S::velocity, {-10.0, 10.0}}}, + }} {}; double dt_ = 1; Eigen::Matrix2d jump_matrix{{0.0, 1.0}, {1.0, 0.0}}; diff --git a/vortex-filtering/test/ipda_test.cpp b/vortex-filtering/test/ipda_test.cpp index d4f2357..ea2eb79 100644 --- a/vortex-filtering/test/ipda_test.cpp +++ b/vortex-filtering/test/ipda_test.cpp @@ -15,8 +15,6 @@ TEST(IPDA, ipda_runs) { .pdaf = { .mahalanobis_threshold = 1.0, - .min_gate_threshold = 0.0, - .max_gate_threshold = 100.0, .prob_of_detection = 0.8, .clutter_intensity = 1.0, }, diff --git a/vortex-filtering/test/pdaf_test.cpp b/vortex-filtering/test/pdaf_test.cpp index 4bcbc95..39673d3 100644 --- a/vortex-filtering/test/pdaf_test.cpp +++ b/vortex-filtering/test/pdaf_test.cpp @@ -195,7 +195,8 @@ TEST(PDAF, apply_gate_is_calculating) { {1.0, 0.0, 1.0, 2.0, 0.0, 2.0}}; // clang-format on - auto gated = PDAF::apply_gate(meas, z_pred, config); + auto gated = + PDAF::apply_gate(meas, z_pred, config.pdaf.mahalanobis_threshold); } TEST(PDAF, apply_gate_is_separating_correctly) { @@ -211,7 +212,8 @@ TEST(PDAF, apply_gate_is_separating_correctly) { {4.0, 0.0}}; // clang-format on - auto gated = PDAF::apply_gate(meas, z_pred, config); + auto gated = + PDAF::apply_gate(meas, z_pred, config.pdaf.mahalanobis_threshold); EXPECT_TRUE(gated(0)); EXPECT_FALSE(gated(1)); @@ -260,7 +262,8 @@ TEST(PDAF, apply_gate_is_separating_correctly_2) { {1.0, 0.0, 1.0, 2.0, 0.0, 2.0}}; // clang-format on - auto gated = PDAF::apply_gate(meas, z_pred, config); + auto gated = + PDAF::apply_gate(meas, z_pred, config.pdaf.mahalanobis_threshold); EXPECT_EQ(gated.count(), 5u);