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 0df43d3..dd9d4d4 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,52 +46,220 @@ class IPDA { }; struct State { - T::Gauss_x x_estimate; + Gauss_x x_estimate; 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 k|k-1. + */ + struct PredictionResult { + /// Predicted state estimate x_{k|k-1} + Gauss_x x_pred; + + /// Predicted measurement distribution z_{k|k-1} + Gauss_z z_pred; + + /// Predicted target existence probability r_{k|k-1} + 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 x_{k|k} + 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 r_k + 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 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; - T::Gauss_x x_prediction; - T::Gauss_z z_prediction; - std::vector x_updates; + + /// Predicted state estimate x_{k|k-1} + Gauss_x x_pred; + + /// Predicted measurement distribution z_{k|k-1} + 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; }; - static double existence_prediction(double existence_prob_est, - double prob_of_survival) { - double r_km1 = existence_prob_est; - double P_s = prob_of_survival; - return P_s * r_km1; // (7.28) + /** + * @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 config Configuration for the IPDA + * @return `PredictionResult` + */ + 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); + + return PredictionResult{.x_pred = x_pred, + .z_pred = z_pred, + .existence_prob_pred = existence_prob_pred}; } /** - * @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. + * @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 `UpdateResult` */ - static double existence_prob_update(const Arr_zXd& z_measurements, - T::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 - 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); + 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); } - // 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; + 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); + + 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_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}; + } + + /** + * @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) { + 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); + + // clang-format off + return { + .state = { + .x_estimate = x_post, + .existence_probability = existence_prob_upd, + }, + .x_pred = x_pred, + .z_pred = z_pred, + .x_updates = x_updates, + .gated_measurements = gated_measurements, + .clutter_intensity = clutter_intensity, + }; + // 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; + double P_s = prob_of_survival; + return P_s * r_km1; // (7.28) } /** @@ -95,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) @@ -114,22 +286,21 @@ 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. + * @param config The configuration for the PDAF. * @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, + const config::PDAF& config) { size_t m_k = num_measurements; - double P_d = config.pdaf.prob_of_detection; - double r_k = predicted_existence_probability; + 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) { @@ -137,49 +308,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..86da919 100644 --- a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp @@ -48,49 +48,161 @@ 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; }; PDAF() = delete; /** - * @brief Perform one step of the Probabilistic Data Association Filter + * @brief Perform one PDAF prediction step * - * @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 + * @return `PredictionResult` + */ + 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 PredictionResult{ + .x_pred = x_pred, + .z_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 `Output` The result of the PDAF step and some intermediate - * results + * @return `UpdateResult` */ - static Output step(const DynModT& dyn_model, - const SensModT& sen_model, - double timestep, - 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); + 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 inside_meas = + auto z_inside_meas = get_inside_measurements(z_measurements, gated_measurements); - 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_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_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}; + 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 UpdateResult{ + .x_post = x_post, + .x_updates = x_updates, + .gated_measurements = gated_measurements, + .z_inside_meas = z_inside_meas, + .z_likelihoods = z_likelihoods, + }; + } + + /** + * @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) { + 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); + return StepResult{ + .x_post = x_post, + .x_pred = x_pred, + .z_pred = z_pred, + .x_updates = x_updates, + .gated_measurements = gated_measurements, + }; } /** @@ -131,30 +243,50 @@ 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; + } + + /** + * @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 * - * @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) { @@ -163,8 +295,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(); } @@ -172,32 +304,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(); diff --git a/vortex-filtering/test/ipda_test.cpp b/vortex-filtering/test/ipda_test.cpp index 03812e5..d4f2357 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 = { @@ -36,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); @@ -65,8 +68,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.pdaf); 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));