From 3e9c23c45492e2b66f9bb8a3535f6995a120b249 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Thu, 11 Dec 2025 20:10:15 +0100 Subject: [PATCH 1/7] predict and update separation --- .../include/vortex_filtering/filters/ipda.hpp | 195 ++++++++++++------ .../include/vortex_filtering/filters/pdaf.hpp | 89 +++++--- 2 files changed, 201 insertions(+), 83 deletions(-) diff --git a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp index 0df43d3..3703799 100644 --- a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp @@ -28,6 +28,10 @@ class IPDA { using T = Types_xzuvw; + using Gauss_z = typename T::Gauss_z; + using Gauss_x = typename T::Gauss_x; + using Gauss_xX = std::vector; + using Arr_zXd = Eigen::Array; using Arr_1Xb = Eigen::Array; using EKF = @@ -42,18 +46,133 @@ class IPDA { }; struct State { - T::Gauss_x x_estimate; + Gauss_x x_estimate; double existence_probability; }; struct Output { State state; - T::Gauss_x x_prediction; - T::Gauss_z z_prediction; - std::vector x_updates; + Gauss_x x_prediction; + Gauss_z z_prediction; + Gauss_xX x_updates; Arr_1Xb gated_measurements; }; + /** + * @brief Perform one IPDA prediction step + * + * @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 `std::tuple` The predicted state, + * predicted measurement, and predicted existence probability + */ + static std::tuple predict( + const DynModT dyn_mod, + const SensModT& sens_mod, + double dt, + const State& state_est_prev, + const Arr_zXd& z_measurements, + Config& config) { + double existence_prob_pred = existence_prediction( + state_est_prev.existence_probability, config.ipda.prob_of_survival); + + auto [x_pred, z_pred] = + EKF::predict(dyn_mod, sens_mod, dt, state_est_prev.x_estimate); + + if (config.ipda.estimate_clutter) { + config.pdaf.clutter_intensity = estimate_clutter_intensity( + z_pred, existence_prob_pred, z_measurements.cols(), config); + } + + return {x_pred, z_pred, existence_prob_pred}; + } + + /** + * @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 + * @return `std::tuple` The + * updated state, all updated states for each measurement, the indices of + * the measurements that are inside the gate, the measurements that are + * inside the gate and the updated existence probability + */ + static std::tuple 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) { + typename PDAF::Config pdaf_cfg{.pdaf = config.pdaf}; + auto [x_post, x_updates, gated_measurements, z_inside_meas] = + PDAF::update(sens_mod, x_pred, z_pred, z_measurements, pdaf_cfg); + + double existence_prob_upd = existence_prob_pred; + if (z_measurements.cols() > 0 || + config.ipda.update_existence_probability_on_no_detection) { + existence_prob_upd = existence_prob_update( + z_inside_meas, z_pred, existence_prob_pred, config); + } + + return std::make_tuple(x_post, x_updates, gated_measurements, + z_inside_meas, existence_prob_upd); + } + + /** + * @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 `Output` The result of the IPDA step and some intermediate + * results + */ + static Output step(const DynModT& dyn_mod, + const SensModT& sens_mod, + double dt, + const State& state_est_prev, + const Arr_zXd& z_measurements, + Config& config) { + auto [x_pred, z_pred, existence_prob_pred] = predict( + dyn_mod, sens_mod, dt, state_est_prev, z_measurements, config); + + auto [x_post, x_updates, gated_measurements, z_inside_meas, + existence_prob_upd] = + update(sens_mod, x_pred, z_pred, z_measurements, + existence_prob_pred, config); + + // clang-format off + return { + .state = { + .x_estimate = x_post, + .existence_probability = existence_prob_upd, + }, + .x_prediction = x_pred, + .z_prediction = z_pred, + .x_updates = x_updates, + .gated_measurements = gated_measurements + }; + // clang-format on + } + + /** + * @brief Calculates the predicted existence probability + * @param existence_prob_est (r_{k-1}) The previous existence probability. + * @param prob_of_survival (P_s) The probability of survival. + * @return The predicted existence probability (r_{k|k-1}). + */ static double existence_prediction(double existence_prob_est, double prob_of_survival) { double r_km1 = existence_prob_est; @@ -62,23 +181,24 @@ class IPDA { } /** - * @brief Calculates the existence probability given the measurements and - * the previous existence probability. + * @brief Calculates the existence probability given the measurements + * and the previous existence probability. * @param z_measurements The measurements to iterate over. * @param z_pred The predicted measurement. - * @param existence_prob_est (r_{k-1}) The previous existence probability. + * @param existence_prob_est (r_{k-1}) The previous existence + * probability. * @param config The configuration for the IPDA. * @return The existence probability. */ static double existence_prob_update(const Arr_zXd& z_measurements, - T::Gauss_z& z_pred, + const Gauss_z& z_pred, double existence_prob_pred, Config config) { double r_kgkm1 = existence_prob_pred; double P_d = config.pdaf.prob_of_detection; double lambda = config.pdaf.clutter_intensity; - // predicted measurement probability + // predicted measurement likelihood sum double z_pred_prob = 0.0; for (const typename T::Vec_z& z_k : z_measurements.colwise()) { z_pred_prob += z_pred.pdf(z_k); @@ -114,20 +234,19 @@ class IPDA { /** * @brief Estimates the clutter intensity using (7.31) * @param z_pred The predicted measurement. - * @param predicted_existence_probability (r_{k|k-1}) The predicted + * @param existence_prob_pred (r_{k|k-1}) The predicted * existence probability. * @param num_measurements (m_k) The number of z_measurements. * @param config The configuration for the IPDA. * @return The clutter intensity. */ - static double estimate_clutter_intensity( - const T::Gauss_z& z_pred, - double predicted_existence_probability, - double num_measurements, - Config config) { + static double estimate_clutter_intensity(const Gauss_z& z_pred, + double existence_prob_pred, + double num_measurements, + Config config) { size_t m_k = num_measurements; double P_d = config.pdaf.prob_of_detection; - double r_k = predicted_existence_probability; + double r_k = existence_prob_pred; double V_k = utils::Ellipsoid(z_pred, config.pdaf.mahalanobis_threshold) .volume(); // gate area @@ -137,49 +256,5 @@ class IPDA { } return 1 / V_k * (m_k - r_k * P_d); // (7.31) } - - static Output step(const DynModT& dyn_mod, - const SensModT& sens_mod, - double timestep, - const State& state_est_prev, - const Arr_zXd& z_measurements, - Config& config) { - double existence_prob_pred = existence_prediction( - state_est_prev.existence_probability, config.ipda.prob_of_survival); - - if (config.ipda.estimate_clutter) { - typename T::Gauss_z z_pred; - std::tie(std::ignore, z_pred) = EKF::predict( - dyn_mod, sens_mod, timestep, state_est_prev.x_estimate); - config.pdaf.clutter_intensity = estimate_clutter_intensity( - z_pred, existence_prob_pred, z_measurements.cols(), config); - } - - auto [x_post, x_pred, z_pred, x_upd, gated_measurements] = - PDAF::step(dyn_mod, sens_mod, timestep, state_est_prev.x_estimate, - z_measurements, {config.pdaf}); - - Arr_zXd z_meas_inside = - PDAF::get_inside_measurements(z_measurements, gated_measurements); - - double existence_probability_upd = existence_prob_pred; - if (!(z_measurements.cols() == 0 && - config.ipda.update_existence_probability_on_no_detection)) { - existence_probability_upd = existence_prob_update( - z_meas_inside, z_pred, existence_prob_pred, config); - } - // clang-format off - return { - .state = { - .x_estimate = x_post, - .existence_probability = existence_probability_upd, - }, - .x_prediction = x_pred, - .z_prediction = z_pred, - .x_updates = x_upd, - .gated_measurements = gated_measurements - }; - // clang-format on - } }; } // namespace vortex::filter diff --git a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp index 1452ab5..99369c3 100644 --- a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp @@ -58,39 +58,82 @@ class PDAF { PDAF() = delete; + /** + * @brief Perform one PDAF prediction step + * + * @param dyn_mod The dynamic model + * @param sens_mod The sensor model + * @param dt Time step in seconds + * @param x_est The estimated state + * @return `std::pair` The predicted state and predicted + * measurement + */ + static std::pair predict(const DynModT& dyn_mod, + const SensModT& sens_mod, + double dt, + const Gauss_x& x_est) { + auto [x_pred, z_pred] = EKF::predict(dyn_mod, sens_mod, dt, x_est); + + return {x_pred, z_pred}; + } + + /** + * @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 `std::tuple` The updated state, all + * updated states for each measurement, and the indices of the measurements + * that are inside the gate + */ + static std::tuple 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); + auto z_inside_meas = + get_inside_measurements(z_measurements, gated_measurements); + + Gauss_xX x_updates; + for (const auto& z_k : z_inside_meas.colwise()) { + x_updates.push_back(EKF::update(sens_mod, x_pred, z_pred, z_k)); + } + + Gauss_x x_post = get_weighted_average( + z_inside_meas, x_updates, z_pred, x_pred, + config.pdaf.prob_of_detection, config.pdaf.clutter_intensity); + + return {x_post, x_updates, gated_measurements, z_inside_meas}; + } + /** * @brief Perform one step of the Probabilistic Data Association Filter * - * @param dyn_model The dynamic model - * @param sen_model The sensor model - * @param timestep Time step in seconds + * @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 `Output` The result of the PDAF step and some intermediate * results */ - static Output step(const DynModT& dyn_model, - const SensModT& sen_model, - double timestep, + static Output step(const DynModT& dyn_mod, + const SensModT& sens_mod, + double dt, const Gauss_x& x_est, const Arr_zXd& z_measurements, const Config& config) { - auto [x_pred, z_pred] = - EKF::predict(dyn_model, sen_model, timestep, x_est); - auto gated_measurements = apply_gate(z_measurements, z_pred, config); - auto inside_meas = - get_inside_measurements(z_measurements, gated_measurements); + auto [x_pred, z_pred] = predict(dyn_mod, sens_mod, dt, x_est); - Gauss_xX x_updated; - for (const auto& z_k : inside_meas.colwise()) { - x_updated.push_back(EKF::update(sen_model, x_pred, z_pred, z_k)); - } - - Gauss_x x_final = get_weighted_average( - inside_meas, x_updated, z_pred, x_pred, - config.pdaf.prob_of_detection, config.pdaf.clutter_intensity); - return {x_final, x_pred, z_pred, x_updated, gated_measurements}; + auto [x_final, x_updates, gated_measurements, z_inside_meas] = + update(sens_mod, x_pred, z_pred, z_measurements, config); + return {x_final, x_pred, z_pred, x_updates, gated_measurements}; } /** @@ -131,14 +174,14 @@ class PDAF { */ static Arr_zXd get_inside_measurements(const Arr_zXd& z_measurements, const Arr_1Xb& gated_measurements) { - Arr_zXd inside_meas(N_DIM_z, gated_measurements.count()); + Arr_zXd z_inside_meas(N_DIM_z, gated_measurements.count()); for (size_t i = 0, j = 0; bool gated : gated_measurements) { if (gated) { - inside_meas.col(j++) = z_measurements.col(i); + z_inside_meas.col(j++) = z_measurements.col(i); } i++; } - return inside_meas; + return z_inside_meas; } /** From 5e873750c103b88e614af875b91078c1de7ac29c Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Fri, 12 Dec 2025 15:26:18 +0100 Subject: [PATCH 2/7] pdaf z_likelihood function --- .../include/vortex_filtering/filters/pdaf.hpp | 22 +++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp index 99369c3..f3c10b8 100644 --- a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp @@ -184,6 +184,28 @@ class PDAF { return z_inside_meas; } + /** + * @brief Get the measurement likelihoods + * + * @param z_pred The predicted measurement + * @param z_measurements The measurements + * + * @return `Eigen::ArrayXd` The measurement likelihoods + */ + static Eigen::ArrayXd get_measurement_likelihoods( + const Gauss_z& z_pred, + const Arr_zXd& z_measurements) { + const int m = z_measurements.cols(); + Eigen::ArrayXd meas_likelihoods(m); + + int i = 0; + for (const Vec_z& z_k : z_measurements.colwise()) { + meas_likelihoods(i++) = z_pred.pdf(z_k); + } + + return meas_likelihoods; + } + /** * @brief Get the weighted average of the states * From da8bde3b5c3e61739dd376e2a76f4e334ae18257 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Fri, 12 Dec 2025 15:27:15 +0100 Subject: [PATCH 3/7] refactor z_likelihood function --- .../include/vortex_filtering/filters/ipda.hpp | 5 +- .../include/vortex_filtering/filters/pdaf.hpp | 69 +++++++++---------- 2 files changed, 35 insertions(+), 39 deletions(-) diff --git a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp index 3703799..9254678 100644 --- a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp @@ -112,14 +112,15 @@ class IPDA { double existence_prob_pred, const Config& config) { typename PDAF::Config pdaf_cfg{.pdaf = config.pdaf}; - auto [x_post, x_updates, gated_measurements, z_inside_meas] = + auto [x_post, x_updates, gated_measurements, z_inside_meas, + z_likelihoods] = PDAF::update(sens_mod, x_pred, z_pred, z_measurements, pdaf_cfg); double existence_prob_upd = existence_prob_pred; if (z_measurements.cols() > 0 || config.ipda.update_existence_probability_on_no_detection) { existence_prob_upd = existence_prob_update( - z_inside_meas, z_pred, existence_prob_pred, config); + z_likelihoods, existence_prob_pred, config); } return std::make_tuple(x_post, x_updates, gated_measurements, diff --git a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp index f3c10b8..08c590b 100644 --- a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp @@ -85,16 +85,16 @@ class PDAF { * @param z_pred The predicted measurement * @param z_measurements Array of measurements * @param config Configuration for the PDAF - * @return `std::tuple` The updated state, all - * updated states for each measurement, and the indices of the measurements - * that are inside the gate + * @return `std::tuple` The + * updated state, all updated states for each measurement, the indices of + * the measurements that are inside the gate and the measurement likelihoods */ - static std::tuple update( - const SensModT& sens_mod, - const Gauss_x& x_pred, - const Gauss_z& z_pred, - const Arr_zXd& z_measurements, - const Config& config) { + static std::tuple + 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); auto z_inside_meas = get_inside_measurements(z_measurements, gated_measurements); @@ -104,11 +104,15 @@ class PDAF { x_updates.push_back(EKF::update(sens_mod, x_pred, z_pred, z_k)); } - Gauss_x x_post = get_weighted_average( - z_inside_meas, x_updates, z_pred, x_pred, - config.pdaf.prob_of_detection, config.pdaf.clutter_intensity); + Eigen::ArrayXd z_likelihoods = + get_measurement_likelihoods(z_pred, z_inside_meas); + + Gauss_x x_post = get_weighted_average(z_likelihoods, x_updates, x_pred, + config.pdaf.prob_of_detection, + config.pdaf.clutter_intensity); - return {x_post, x_updates, gated_measurements, z_inside_meas}; + return {x_post, x_updates, gated_measurements, z_inside_meas, + z_likelihoods}; } /** @@ -131,7 +135,8 @@ class PDAF { const Config& config) { auto [x_pred, z_pred] = predict(dyn_mod, sens_mod, dt, x_est); - auto [x_final, x_updates, gated_measurements, z_inside_meas] = + auto [x_final, x_updates, gated_measurements, z_inside_meas, + z_likelihoods] = update(sens_mod, x_pred, z_pred, z_measurements, config); return {x_final, x_pred, z_pred, x_updates, gated_measurements}; } @@ -209,17 +214,15 @@ class PDAF { /** * @brief Get the weighted average of the states * - * @param z_measurements Array of measurements + * @param z_likelihoods Array of measurement likelihoods * @param updated_states Array of updated states - * @param z_pred Predicted measurement * @param x_pred Predicted state * @param prob_of_detection Probability of detection * @param clutter_intensity Clutter intensity * @return `Gauss_x` The weighted average of the states */ - static Gauss_x get_weighted_average(const Arr_zXd& z_measurements, + static Gauss_x get_weighted_average(const Eigen::ArrayXd& z_likelihoods, const Gauss_xX& updated_states, - const Gauss_z& z_pred, const Gauss_x& x_pred, double prob_of_detection, double clutter_intensity) { @@ -228,8 +231,8 @@ class PDAF { states.insert(states.end(), updated_states.begin(), updated_states.end()); - Eigen::VectorXd weights = get_weights( - z_measurements, z_pred, prob_of_detection, clutter_intensity); + Eigen::VectorXd weights = + get_weights(z_likelihoods, prob_of_detection, clutter_intensity); return GaussMix_x{weights, states}.reduce(); } @@ -237,32 +240,24 @@ class PDAF { /** * @brief Get the weights for the measurements * - * @param z_measurements Array of measurements - * @param z_pred Predicted measurement + * @param z_likelihoods Array of measurements * @param prob_of_detection Probability of detection * @param clutter_intensity Clutter intensity * @return `Eigen::VectorXd` The weights for the measurements */ - static Eigen::VectorXd get_weights(const Arr_zXd& z_measurements, - const Gauss_z& z_pred, + static Eigen::VectorXd get_weights(const Eigen::ArrayXd& z_likelihoods, double prob_of_detection, double clutter_intensity) { - double lambda = clutter_intensity; - double P_d = prob_of_detection; + const int m = z_likelihoods.size(); + const double lambda = clutter_intensity; + const double P_d = prob_of_detection; - Eigen::VectorXd weights(z_measurements.cols() + 1); + Eigen::VectorXd weights(m + 1); - // in case no measurement associates with the target - if (lambda == 0.0 && z_measurements.cols() == 0) { - weights(0) = 1.0; - } else { - weights(0) = lambda * (1 - P_d); - } + // Weight for "no measurement associated" + weights(0) = (lambda == 0.0 && m == 0) ? 1.0 : lambda * (1.0 - P_d); - // measurements associating with the target - for (size_t a_k = 1; const Vec_z& z_k : z_measurements.colwise()) { - weights(a_k++) = P_d * z_pred.pdf(z_k); - } + weights.tail(m) = P_d * z_likelihoods; // normalize weights weights /= weights.sum(); From ba22867aedb5bd51cf721a0c25436ce1c5953d42 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Fri, 12 Dec 2025 15:27:40 +0100 Subject: [PATCH 4/7] remove unsused function --- .../include/vortex_filtering/filters/ipda.hpp | 30 ------------------- 1 file changed, 30 deletions(-) diff --git a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp index 9254678..a2c2d3e 100644 --- a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp @@ -181,36 +181,6 @@ class IPDA { return P_s * r_km1; // (7.28) } - /** - * @brief Calculates the existence probability given the measurements - * and the previous existence probability. - * @param z_measurements The measurements to iterate over. - * @param z_pred The predicted measurement. - * @param existence_prob_est (r_{k-1}) The previous existence - * probability. - * @param config The configuration for the IPDA. - * @return The existence probability. - */ - static double existence_prob_update(const Arr_zXd& z_measurements, - const Gauss_z& z_pred, - double existence_prob_pred, - Config config) { - double r_kgkm1 = existence_prob_pred; - double P_d = config.pdaf.prob_of_detection; - double lambda = config.pdaf.clutter_intensity; - - // predicted measurement likelihood sum - double z_pred_prob = 0.0; - for (const typename T::Vec_z& z_k : z_measurements.colwise()) { - z_pred_prob += z_pred.pdf(z_k); - } - - // posterior existence probability r_k - double L_k = 1 - P_d + P_d / lambda * z_pred_prob; // (7.33) - double r_k = (L_k * r_kgkm1) / (1 - (1 - L_k) * r_kgkm1); // (7.32) - return r_k; - } - /** * @brief Calculates the existence probability given the likelihood of the * measurements and the previous existence probability. From 9c2f892a60284191fc473bd34c50e58c1138aa22 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Fri, 12 Dec 2025 15:28:18 +0100 Subject: [PATCH 5/7] fix tests after refactor --- vortex-filtering/test/ipda_test.cpp | 7 +++++- vortex-filtering/test/pdaf_test.cpp | 35 +++++++++++++++++++++++------ 2 files changed, 34 insertions(+), 8 deletions(-) diff --git a/vortex-filtering/test/ipda_test.cpp b/vortex-filtering/test/ipda_test.cpp index 03812e5..a61a3cb 100644 --- a/vortex-filtering/test/ipda_test.cpp +++ b/vortex-filtering/test/ipda_test.cpp @@ -2,11 +2,13 @@ #include #include #include +#include #include using ConstantVelocity = vortex::models::ConstantVelocity; using IdentitySensorModel = vortex::models::IdentitySensorModel<4, 2>; using IPDA = vortex::filter::IPDA; +using PDAF = vortex::filter::PDAF; TEST(IPDA, ipda_runs) { IPDA::Config config = { @@ -65,8 +67,11 @@ TEST(IPDA, get_existence_probability_is_calculating) { vortex::prob::Gauss2d z_pred = {{1.0, 1.0}, Eigen::Matrix2d::Identity() * 0.1}; + Eigen::ArrayXd z_likelihoods = + PDAF::get_measurement_likelihoods(z_pred, meas); + double existence_probability = IPDA::existence_prob_update( - meas, z_pred, last_detection_probability, config); + z_likelihoods, last_detection_probability, config); std::cout << "Existence probability: " << existence_probability << std::endl; diff --git a/vortex-filtering/test/pdaf_test.cpp b/vortex-filtering/test/pdaf_test.cpp index d16d8cc..4bcbc95 100644 --- a/vortex-filtering/test/pdaf_test.cpp +++ b/vortex-filtering/test/pdaf_test.cpp @@ -17,8 +17,11 @@ TEST(PDAF, get_weights_is_calculating) { Eigen::Matrix2d::Identity()); Eigen::Array2Xd meas = {{0.0, 2.0}, {0.0, 1.0}}; + Eigen::ArrayXd z_likelihoods = + PDAF::get_measurement_likelihoods(z_pred, meas); + Eigen::VectorXd weights = - PDAF::get_weights(meas, z_pred, prob_of_detection, clutter_intensity); + PDAF::get_weights(z_likelihoods, prob_of_detection, clutter_intensity); std::cout << "weights: " << weights << std::endl; @@ -33,8 +36,11 @@ TEST(PDAF, if_no_clutter_first_weight_is_zero) { Eigen::Matrix2d::Identity()); Eigen::Array2Xd meas = {{0.0, 2.0}, {0.0, 1.0}}; + Eigen::ArrayXd z_likelihoods = + PDAF::get_measurement_likelihoods(z_pred, meas); + Eigen::VectorXd weights = - PDAF::get_weights(meas, z_pred, prob_of_detection, clutter_intensity); + PDAF::get_weights(z_likelihoods, prob_of_detection, clutter_intensity); std::cout << "weights: " << weights << std::endl; @@ -49,8 +55,11 @@ TEST(PDAF, weights_are_decreasing_with_distance) { Eigen::Matrix2d::Identity()); Eigen::Array2Xd meas = {{2.0, 3.0, 4.0}, {1.0, 1.0, 1.0}}; + Eigen::ArrayXd z_likelihoods = + PDAF::get_measurement_likelihoods(z_pred, meas); + Eigen::VectorXd weights = - PDAF::get_weights(meas, z_pred, prob_of_detection, clutter_intensity); + PDAF::get_weights(z_likelihoods, prob_of_detection, clutter_intensity); std::cout << "weights: " << weights << std::endl; @@ -74,8 +83,11 @@ TEST(PDAF, get_weighted_average_is_calculating) { vortex::prob::Gauss4d(Eigen::Vector4d(1.0, 1.0, 1.0, 1.0), Eigen::Matrix4d::Identity())}; + Eigen::ArrayXd z_likelihoods = + PDAF::get_measurement_likelihoods(z_pred, meas); + vortex::prob::Gauss4d weighted_average = - PDAF::get_weighted_average(meas, updated_states, z_pred, x_pred, + PDAF::get_weighted_average(z_likelihoods, updated_states, x_pred, prob_of_detection, clutter_intensity); std::cout << "weighted average: " << weighted_average.mean() << std::endl; @@ -94,8 +106,11 @@ TEST(PDAF, average_state_is_in_between_prediction_and_measurement_y_axis) { std::vector updated_states = {vortex::prob::Gauss4d( Eigen::Vector4d(1.0, 1.5, 0.0, 0.0), Eigen::Matrix4d::Identity())}; + Eigen::ArrayXd z_likelihoods = + PDAF::get_measurement_likelihoods(z_pred, meas); + vortex::prob::Gauss4d weighted_average = - PDAF::get_weighted_average(meas, updated_states, z_pred, x_pred, + PDAF::get_weighted_average(z_likelihoods, updated_states, x_pred, prob_of_detection, clutter_intensity); EXPECT_GT(weighted_average.mean()(1), x_pred.mean()(1)); @@ -119,8 +134,11 @@ TEST(PDAF, average_state_is_in_between_prediction_and_measurement_x_axis) { std::vector updated_states = {vortex::prob::Gauss4d( Eigen::Vector4d(1.5, 1.0, 0.0, 0.0), Eigen::Matrix4d::Identity())}; + Eigen::ArrayXd z_likelihoods = + PDAF::get_measurement_likelihoods(z_pred, meas); + vortex::prob::Gauss4d weighted_average = - PDAF::get_weighted_average(meas, updated_states, z_pred, x_pred, + PDAF::get_weighted_average(z_likelihoods, updated_states, x_pred, prob_of_detection, clutter_intensity); EXPECT_GT(weighted_average.mean()(0), x_pred.mean()(0)); @@ -144,8 +162,11 @@ TEST(PDAF, average_state_is_in_between_prediction_and_measurement_both_axes) { std::vector updated_states = {vortex::prob::Gauss4d( Eigen::Vector4d(1.5, 1.5, 0.0, 0.0), Eigen::Matrix4d::Identity())}; + Eigen::ArrayXd z_likelihoods = + PDAF::get_measurement_likelihoods(z_pred, meas); + vortex::prob::Gauss4d weighted_average = - PDAF::get_weighted_average(meas, updated_states, z_pred, x_pred, + PDAF::get_weighted_average(z_likelihoods, updated_states, x_pred, prob_of_detection, clutter_intensity); EXPECT_GT(weighted_average.mean()(0), x_pred.mean()(0)); From fa86bfca65c31af55bc1efc586b250013d842ba4 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Mon, 15 Dec 2025 20:20:34 +0100 Subject: [PATCH 6/7] Structured return types. Const config passing --- .../vortex_filtering/filters/immipda.hpp | 3 +- .../include/vortex_filtering/filters/ipda.hpp | 191 +++++++++++++----- .../include/vortex_filtering/filters/pdaf.hpp | 126 +++++++++--- vortex-filtering/test/ipda_test.cpp | 5 +- 4 files changed, 235 insertions(+), 90 deletions(-) diff --git a/vortex-filtering/include/vortex_filtering/filters/immipda.hpp b/vortex-filtering/include/vortex_filtering/filters/immipda.hpp index aca08ef..43541a0 100644 --- a/vortex-filtering/include/vortex_filtering/filters/immipda.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/immipda.hpp @@ -218,8 +218,7 @@ class IMMIPDA { // Calculate existence probability (7.32-7.33) double existence_prob_upd = IPDA_<0>::existence_prob_update( - hypothesis_likelihoods, existence_prob_est, - {config.pdaf, config.ipda}); + hypothesis_likelihoods, existence_prob_est, config.pdaf); return { .state = diff --git a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp index a2c2d3e..931291a 100644 --- a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp @@ -50,12 +50,89 @@ class IPDA { double existence_probability; }; - struct Output { + /** + * @brief Result of the IPDA prediction step. + * + * Contains the predicted kinematic state, predicted measurement, and + * predicted target existence probability. + * + * @note + * Corresponds to quantities at time step \f$k|k-1\f$. + */ + struct PredictionResult { + /// Predicted state estimate \f$x_{k|k-1}\f$ + Gauss_x x_pred; + + /// Predicted measurement distribution \f$z_{k|k-1}\f$ + Gauss_z z_pred; + + /// Predicted target existence probability \f$r_{k|k-1}\f$ + double existence_prob_pred; + }; + + /** + * @brief Result of the IPDA update step. + * + * Contains the posterior state estimate after data association, the + * individual measurement-conditioned updates, gating results, and the + * updated target existence probability. + * + * @note + * - `x_updates[i]` corresponds to `z_inside_meas.col(i)` + * - `clutter_intensity` may be estimated or fixed depending on + * configuration + */ + struct UpdateResult { + /// Posterior state estimate after IPDA update \f$x_{k|k}\f$ + Gauss_x x_post; + + /// State updates conditioned on each gated measurement + Gauss_xX x_updates; + + /// Boolean mask indicating which measurements passed the validation + /// gate + Arr_1Xb gated_measurements; + + /// Measurements inside the validation gate + Arr_zXd z_inside_meas; + + /// Updated target existence probability \f$r_k\f$ + double existence_prob_upd; + + /// Clutter intensity used during the update + double clutter_intensity; + }; + + /** + * @brief Result of a complete IPDA filter step. + * + * Aggregates prediction and update results into a single output structure, + * suitable for external use, logging, or higher-level filters (e.g. IMM). + * + * @note + * - `state.x_estimate` corresponds to \f$x_{k|k}\f$ + * - `x_pred` corresponds to \f$x_{k|k-1}\f$ + * - `clutter_intensity` is the value actually used in the PDAF update + */ + struct StepResult { + /// Updated target state and existence probability State state; - Gauss_x x_prediction; - Gauss_z z_prediction; + + /// Predicted state estimate \f$x_{k|k-1}\f$ + Gauss_x x_pred; + + /// Predicted measurement distribution \f$z_{k|k-1}\f$ + Gauss_z z_pred; + + /// State updates conditioned on each gated measurement Gauss_xX x_updates; + + /// Boolean mask indicating which measurements passed the validation + /// gate Arr_1Xb gated_measurements; + + /// Clutter intensity used during the update step + double clutter_intensity; }; /** @@ -65,30 +142,23 @@ class IPDA { * @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 `std::tuple` The predicted state, - * predicted measurement, and predicted existence probability + * @return `PredictionResult` */ - static std::tuple predict( - const DynModT dyn_mod, - const SensModT& sens_mod, - double dt, - const State& state_est_prev, - const Arr_zXd& z_measurements, - Config& config) { + static PredictionResult predict(const DynModT& dyn_mod, + const SensModT& sens_mod, + double dt, + const State& state_est_prev, + const Config& config) { double existence_prob_pred = existence_prediction( state_est_prev.existence_probability, config.ipda.prob_of_survival); auto [x_pred, z_pred] = EKF::predict(dyn_mod, sens_mod, dt, state_est_prev.x_estimate); - if (config.ipda.estimate_clutter) { - config.pdaf.clutter_intensity = estimate_clutter_intensity( - z_pred, existence_prob_pred, z_measurements.cols(), config); - } - - return {x_pred, z_pred, existence_prob_pred}; + return PredictionResult{.x_pred = x_pred, + .z_pred = z_pred, + .existence_prob_pred = existence_prob_pred}; } /** @@ -99,19 +169,25 @@ class IPDA { * @param z_measurements Array of measurements * @param existence_prob_pred The predicted existence probability * @param config Configuration for the IPDA - * @return `std::tuple` The - * updated state, all updated states for each measurement, the indices of - * the measurements that are inside the gate, the measurements that are - * inside the gate and the updated existence probability + * @return `UpdateResult` */ - static std::tuple 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) { + 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) { + double clutter_intensity = config.pdaf.clutter_intensity; + + if (config.ipda.estimate_clutter) { + clutter_intensity = + estimate_clutter_intensity(z_pred, existence_prob_pred, + z_measurements.cols(), config.pdaf); + } + typename PDAF::Config pdaf_cfg{.pdaf = config.pdaf}; + pdaf_cfg.pdaf.clutter_intensity = clutter_intensity; + auto [x_post, x_updates, gated_measurements, z_inside_meas, z_likelihoods] = PDAF::update(sens_mod, x_pred, z_pred, z_measurements, pdaf_cfg); @@ -120,11 +196,15 @@ class IPDA { if (z_measurements.cols() > 0 || config.ipda.update_existence_probability_on_no_detection) { existence_prob_upd = existence_prob_update( - z_likelihoods, existence_prob_pred, config); + z_likelihoods, existence_prob_pred, pdaf_cfg.pdaf); } - return std::make_tuple(x_post, x_updates, gated_measurements, - z_inside_meas, existence_prob_upd); + 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}; } /** @@ -137,20 +217,20 @@ class IPDA { * @param state_est_prev The previous estimated state * @param z_measurements Array of measurements * @param config Configuration for the IPDA - * @return `Output` The result of the IPDA step and some intermediate + * @return `StepResult` The result of the IPDA step and some intermediate * results */ - static Output step(const DynModT& dyn_mod, - const SensModT& sens_mod, - double dt, - const State& state_est_prev, - const Arr_zXd& z_measurements, - Config& config) { - auto [x_pred, z_pred, existence_prob_pred] = predict( - dyn_mod, sens_mod, dt, state_est_prev, z_measurements, config); + 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) { + 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] = + existence_prob_upd, clutter_intensity] = update(sens_mod, x_pred, z_pred, z_measurements, existence_prob_pred, config); @@ -160,10 +240,11 @@ class IPDA { .x_estimate = x_post, .existence_probability = existence_prob_upd, }, - .x_prediction = x_pred, - .z_prediction = z_pred, + .x_pred = x_pred, + .z_pred = z_pred, .x_updates = x_updates, - .gated_measurements = gated_measurements + .gated_measurements = gated_measurements, + .clutter_intensity = clutter_intensity, }; // clang-format on } @@ -186,15 +267,15 @@ class IPDA { * measurements and the previous existence probability. * @param z_likelyhoods (l_a_k) The likelihood of the measurements * @param existence_prob_est (r_{k-1}) The previous existence probability. - * @param config The configuration for the IPDA. + * @param config The configuration for the PDAF. * @return The existence probability (r_k). */ - static double existence_prob_update(const Eigen::ArrayXd z_likelyhoods, + static double existence_prob_update(const Eigen::ArrayXd& z_likelyhoods, double existence_prob_pred, - Config config) { + const config::PDAF& config) { double r_kgkm1 = existence_prob_pred; // r_k given k minus 1 - double P_d = config.pdaf.prob_of_detection; - double lambda = config.pdaf.clutter_intensity; + double P_d = config.prob_of_detection; + double lambda = config.clutter_intensity; // posterior existence probability r_k double L_k = 1 - P_d + P_d / lambda * z_likelyhoods.sum(); // (7.33) @@ -208,18 +289,18 @@ class IPDA { * @param existence_prob_pred (r_{k|k-1}) The predicted * existence probability. * @param num_measurements (m_k) The number of z_measurements. - * @param config The configuration for the IPDA. + * @param config The configuration for the PDAF. * @return The clutter intensity. */ static double estimate_clutter_intensity(const Gauss_z& z_pred, double existence_prob_pred, double num_measurements, - Config config) { + const config::PDAF& config) { size_t m_k = num_measurements; - double P_d = config.pdaf.prob_of_detection; + double P_d = config.prob_of_detection; double r_k = existence_prob_pred; double V_k = - utils::Ellipsoid(z_pred, config.pdaf.mahalanobis_threshold) + utils::Ellipsoid(z_pred, config.mahalanobis_threshold) .volume(); // gate area if (m_k == 0) { diff --git a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp index 08c590b..86da919 100644 --- a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp @@ -48,11 +48,65 @@ class PDAF { config::PDAF pdaf; }; - struct Output { - Gauss_x x_; - Gauss_x x_prediction; - Gauss_z z_prediction; + /** + * @brief Result of the PDAF prediction step. + * + * Contains the predicted state and the corresponding predicted measurement + * prior to processing any measurements at the current time step. + */ + struct PredictionResult { + /// Predicted state distribution x_{k|k-1} + Gauss_x x_pred; + + /// Predicted measurement distribution z_{k|k-1} + Gauss_z z_pred; + }; + + /** + * @brief Result of the PDAF update step. + * + * Contains the posterior state estimate after probabilistic data + * association, as well as intermediate results used during the update. + */ + struct UpdateResult { + /// Posterior state estimate x_{k|k} + Gauss_x x_post; + + /// State updates corresponding to each gated measurement + Gauss_xX x_updates; + + /// Boolean mask indicating which measurements passed the validation + /// gate + Arr_1Xb gated_measurements; + + /// Measurements that passed the validation gate + Arr_zXd z_inside_meas; + + /// Likelihood of each gated measurement p(z_k | x_{k|k-1}) + Eigen::ArrayXd z_likelihoods; + }; + + /** + * @brief Result of a complete PDAF cycle (prediction + update). + * + * Combines the final posterior estimate with selected intermediate results + * from the prediction and update steps. + */ + struct StepResult { + /// Posterior state estimate after the update x_{k|k} + Gauss_x x_post; + + /// Predicted state prior to update x_{k|k-1} + Gauss_x x_pred; + + /// Predicted measurement prior to update z_{k|k-1} + Gauss_z z_pred; + + /// State updates corresponding to each gated measurement Gauss_xX x_updates; + + /// Boolean mask indicating which measurements passed the validation + /// gate Arr_1Xb gated_measurements; }; @@ -65,16 +119,18 @@ class PDAF { * @param sens_mod The sensor model * @param dt Time step in seconds * @param x_est The estimated state - * @return `std::pair` The predicted state and predicted - * measurement + * @return `PredictionResult` */ - static std::pair predict(const DynModT& dyn_mod, - const SensModT& sens_mod, - double dt, - const Gauss_x& x_est) { + static PredictionResult predict(const DynModT& dyn_mod, + const SensModT& sens_mod, + double dt, + const Gauss_x& x_est) { auto [x_pred, z_pred] = EKF::predict(dyn_mod, sens_mod, dt, x_est); - return {x_pred, z_pred}; + return PredictionResult{ + .x_pred = x_pred, + .z_pred = z_pred, + }; } /** @@ -85,16 +141,13 @@ class PDAF { * @param z_pred The predicted measurement * @param z_measurements Array of measurements * @param config Configuration for the PDAF - * @return `std::tuple` The - * updated state, all updated states for each measurement, the indices of - * the measurements that are inside the gate and the measurement likelihoods + * @return `UpdateResult` */ - static std::tuple - update(const SensModT& sens_mod, - const Gauss_x& x_pred, - const Gauss_z& z_pred, - const Arr_zXd& z_measurements, - const Config& config) { + 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); auto z_inside_meas = get_inside_measurements(z_measurements, gated_measurements); @@ -111,8 +164,13 @@ class PDAF { config.pdaf.prob_of_detection, config.pdaf.clutter_intensity); - return {x_post, x_updates, gated_measurements, z_inside_meas, - z_likelihoods}; + return UpdateResult{ + .x_post = x_post, + .x_updates = x_updates, + .gated_measurements = gated_measurements, + .z_inside_meas = z_inside_meas, + .z_likelihoods = z_likelihoods, + }; } /** @@ -124,21 +182,27 @@ class PDAF { * @param x_est The estimated state * @param z_measurements Array of measurements * @param config Configuration for the PDAF - * @return `Output` The result of the PDAF step and some intermediate + * @return `StepResult` The result of the PDAF step and some intermediate * results */ - static Output step(const DynModT& dyn_mod, - const SensModT& sens_mod, - double dt, - const Gauss_x& x_est, - const Arr_zXd& z_measurements, - const Config& config) { + 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) { auto [x_pred, z_pred] = predict(dyn_mod, sens_mod, dt, x_est); - auto [x_final, x_updates, gated_measurements, z_inside_meas, + auto [x_post, x_updates, gated_measurements, z_inside_meas, z_likelihoods] = update(sens_mod, x_pred, z_pred, z_measurements, config); - return {x_final, x_pred, z_pred, x_updates, gated_measurements}; + return StepResult{ + .x_post = x_post, + .x_pred = x_pred, + .z_pred = z_pred, + .x_updates = x_updates, + .gated_measurements = gated_measurements, + }; } /** diff --git a/vortex-filtering/test/ipda_test.cpp b/vortex-filtering/test/ipda_test.cpp index a61a3cb..d4f2357 100644 --- a/vortex-filtering/test/ipda_test.cpp +++ b/vortex-filtering/test/ipda_test.cpp @@ -38,7 +38,8 @@ TEST(IPDA, ipda_runs) { ConstantVelocity dyn_model{1.0}; IdentitySensorModel sen_model{1.0}; - auto [state_post, x_pred, z_pred, x_updated, gated_measurements] = + auto [state_post, x_pred, z_pred, x_updated, gated_measurements, + clutter_intensity] = IPDA::step(dyn_model, sen_model, 1.0, {x_est, last_detection_probability}, z_meas, config); @@ -71,7 +72,7 @@ TEST(IPDA, get_existence_probability_is_calculating) { PDAF::get_measurement_likelihoods(z_pred, meas); double existence_probability = IPDA::existence_prob_update( - z_likelihoods, last_detection_probability, config); + z_likelihoods, last_detection_probability, config.pdaf); std::cout << "Existence probability: " << existence_probability << std::endl; From 2d81e5e8f7780b468efbc651494a54d4047ed9b6 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Mon, 15 Dec 2025 20:22:29 +0100 Subject: [PATCH 7/7] fix doxygen format error --- .../include/vortex_filtering/filters/ipda.hpp | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp index 931291a..dd9d4d4 100644 --- a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp @@ -57,16 +57,16 @@ class IPDA { * predicted target existence probability. * * @note - * Corresponds to quantities at time step \f$k|k-1\f$. + * Corresponds to quantities at time step k|k-1. */ struct PredictionResult { - /// Predicted state estimate \f$x_{k|k-1}\f$ + /// Predicted state estimate x_{k|k-1} Gauss_x x_pred; - /// Predicted measurement distribution \f$z_{k|k-1}\f$ + /// Predicted measurement distribution z_{k|k-1} Gauss_z z_pred; - /// Predicted target existence probability \f$r_{k|k-1}\f$ + /// Predicted target existence probability r_{k|k-1} double existence_prob_pred; }; @@ -83,7 +83,7 @@ class IPDA { * configuration */ struct UpdateResult { - /// Posterior state estimate after IPDA update \f$x_{k|k}\f$ + /// Posterior state estimate after IPDA update x_{k|k} Gauss_x x_post; /// State updates conditioned on each gated measurement @@ -96,7 +96,7 @@ class IPDA { /// Measurements inside the validation gate Arr_zXd z_inside_meas; - /// Updated target existence probability \f$r_k\f$ + /// Updated target existence probability r_k double existence_prob_upd; /// Clutter intensity used during the update @@ -110,18 +110,18 @@ class IPDA { * suitable for external use, logging, or higher-level filters (e.g. IMM). * * @note - * - `state.x_estimate` corresponds to \f$x_{k|k}\f$ - * - `x_pred` corresponds to \f$x_{k|k-1}\f$ + * - `state.x_estimate` corresponds to x_{k|k} + * - `x_pred` corresponds to x_{k|k-1} * - `clutter_intensity` is the value actually used in the PDAF update */ struct StepResult { /// Updated target state and existence probability State state; - /// Predicted state estimate \f$x_{k|k-1}\f$ + /// Predicted state estimate x_{k|k-1} Gauss_x x_pred; - /// Predicted measurement distribution \f$z_{k|k-1}\f$ + /// Predicted measurement distribution z_{k|k-1} Gauss_z z_pred; /// State updates conditioned on each gated measurement