From 3d7b49fedd7748b270785b05e6e796b1ae8a2274 Mon Sep 17 00:00:00 2001 From: TristanWolfram Date: Fri, 12 Jan 2024 11:24:07 +0100 Subject: [PATCH 01/31] Created new branch feature/pdaf --- .../include/vortex_filtering/filters/pdaf.hpp | 138 ++++++++++++++++++ vortex-filtering/test/pdaf_test.cpp | 21 +++ 2 files changed, 159 insertions(+) create mode 100644 vortex-filtering/include/vortex_filtering/filters/pdaf.hpp create mode 100644 vortex-filtering/test/pdaf_test.cpp diff --git a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp new file mode 100644 index 00000000..9d45bbb0 --- /dev/null +++ b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp @@ -0,0 +1,138 @@ +#include +#include +#include +#include +#include +#include + +using std::string; +// using std::endl; + +template +class PDAF { +public: + using SensModI = typename SensModT::SensModI; + using DynModI = typename DynModT::BaseI; + using DynModPtr = std::shared_ptr; + using SensModPtr = std::shared_ptr; + using EKF = vortex::filter::EKF_M; + using Gauss_z = typename SensModI::Gauss_z; + using Gauss_x = typename DynModI::Gauss_x; + using Vec_z = typename SensModI::Vec_z; + using MeasurementsZd = std::vector; + using StatesZd = std::vector; + using GaussMixZd = vortex::prob::GaussianMixture; + + double gate_threshold_; + double prob_of_detection_; + double clutter_intensity_; + + PDAF(double gate, double prob_of_detection, double clutter_intensity) + : gate_threshold_(gate) + , prob_of_detection_(prob_of_detection) + , clutter_intensity_(clutter_intensity) + { + std::cout << "Created PDAF class with given models!" << std::endl; + } + + PDAF() = default; + + // predict next state, if ekf is not defined + std::tuple predict_next_state(Gauss_x x_est, MeasurementsZd z_meas, double timestep, DynModPtr dyn_model, SensModPtr sen_model) + { + EKF ekf; + // std::pair x_z_pred = ekf.predict(dyn_model, sen_model, timestep, x_est); + auto [x_pred, z_pred] = ekf.predict(dyn_model, sen_model, timestep, x_est); + auto [inside, outside] = apply_gate(gate_threshold_, z_meas, z_pred); + + StatesZd updated; + // x_post -> vector + for (const auto& measurement : inside) { + // x_post append with: + updated.push_back(ekf.update(x_pred, z_pred, measurement)); + } + + Gauss_z predicted_state = get_weighted_average(z_meas, updated, z_pred, x_pred); + return {predicted_state, inside, outside}; + } + + std::tuple apply_gate(double gate_threshold, MeasurementsZd z_meas, Gauss_z z_pred) + { + MeasurementsZd inside_meas; + MeasurementsZd outside_meas; + + for (const auto& measurement : z_meas) { + double distance = z_pred.mahalanobis_distance(measurement); + + if (distance < gate_threshold) { + inside_meas.push_back(measurement); + } else { + outside_meas.push_back(measurement); + } + } + + return {inside_meas, outside_meas}; + } + + // Getting weighted average of the predicted states + Gauss_x get_weighted_average(MeasurementsZd z_meas, StatesZd updated_states, Gauss_z z_pred, Gauss_x x_pred) + { + StatesZd states; + states.push_back(x_pred); + states.insert(states.end(), updated_states.begin(), updated_states.end()); + + Eigen::VectorXd weights = get_weights(z_meas, updated_states, z_pred); + + GaussMixZd gaussian_mixture(weights, states); + + return gaussian_mixture.reduce(); + } + + // Getting association probabilities according to textbook p. 123 "Corollary 7.3.3" + Eigen::VectorXd get_weights(MeasurementsZd z_meas, StatesZd updated_states, Gauss_z z_pred) + { + Eigen::VectorXd weights(z_meas.size() + 1); + + double no_association = clutter_intensity_ * (1 - prob_of_detection_); + weights(0) = no_association; + + for (size_t k = 1; k < z_meas.size() + 1; k++) { + weights(k) = (prob_of_detection_ * z_pred.pdf(z_meas.at(k))); + } + + weights /= weights.sum(); + + return weights; + } + + // Getter and setter for mem + void set_gate_threshold(double gate_threshold) + { + gate_threshold_ = gate_threshold; + } + + void set_prob_of_detection(double prob_of_detection) + { + prob_of_detection_ = prob_of_detection; + } + + void set_clutter_intensity(double clutter_intensity) + { + clutter_intensity_ = clutter_intensity; + } + + double get_gate_threshold() + { + return gate_threshold_; + } + + double get_prob_of_detection() + { + return prob_of_detection_; + } + + double get_clutter_intensity() + { + return clutter_intensity_; + } +}; \ No newline at end of file diff --git a/vortex-filtering/test/pdaf_test.cpp b/vortex-filtering/test/pdaf_test.cpp new file mode 100644 index 00000000..8236a6ce --- /dev/null +++ b/vortex-filtering/test/pdaf_test.cpp @@ -0,0 +1,21 @@ +#include +#include +#include + +using SimplePDAF = PDAF, vortex::models::IdentitySensorModel<4, 2>>; + +TEST(PDAF, init) +{ + SimplePDAF pdaf; + EXPECT_EQ(pdaf.gate_threshold_, 0.0); + EXPECT_EQ(pdaf.prob_of_detection_, 0.0); + EXPECT_EQ(pdaf.clutter_intensity_, 0.0); +} + +TEST(PDAF, init_with_params) +{ + SimplePDAF pdaf(1.0, 0.9, 0.1); + EXPECT_EQ(pdaf.gate_threshold_, 1.0); + EXPECT_EQ(pdaf.prob_of_detection_, 0.9); + EXPECT_EQ(pdaf.clutter_intensity_, 0.1); +} \ No newline at end of file From f92bc8c3cd5694860b84e184225770c19cf3fe0c Mon Sep 17 00:00:00 2001 From: TristanWolfram Date: Fri, 12 Jan 2024 15:49:19 +0100 Subject: [PATCH 02/31] Finalized PDAF class and added some tests --- .../include/vortex_filtering/filters/pdaf.hpp | 23 +++-- vortex-filtering/test/pdaf_test.cpp | 87 ++++++++++++++++++- 2 files changed, 96 insertions(+), 14 deletions(-) diff --git a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp index 9d45bbb0..a17e84f0 100644 --- a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp @@ -20,7 +20,7 @@ class PDAF { using Gauss_x = typename DynModI::Gauss_x; using Vec_z = typename SensModI::Vec_z; using MeasurementsZd = std::vector; - using StatesZd = std::vector; + using StatesXd = std::vector; using GaussMixZd = vortex::prob::GaussianMixture; double gate_threshold_; @@ -35,20 +35,14 @@ class PDAF { std::cout << "Created PDAF class with given models!" << std::endl; } - PDAF() = default; - - // predict next state, if ekf is not defined std::tuple predict_next_state(Gauss_x x_est, MeasurementsZd z_meas, double timestep, DynModPtr dyn_model, SensModPtr sen_model) { EKF ekf; - // std::pair x_z_pred = ekf.predict(dyn_model, sen_model, timestep, x_est); auto [x_pred, z_pred] = ekf.predict(dyn_model, sen_model, timestep, x_est); auto [inside, outside] = apply_gate(gate_threshold_, z_meas, z_pred); - StatesZd updated; - // x_post -> vector + StatesXd updated; for (const auto& measurement : inside) { - // x_post append with: updated.push_back(ekf.update(x_pred, z_pred, measurement)); } @@ -75,13 +69,13 @@ class PDAF { } // Getting weighted average of the predicted states - Gauss_x get_weighted_average(MeasurementsZd z_meas, StatesZd updated_states, Gauss_z z_pred, Gauss_x x_pred) + Gauss_x get_weighted_average(MeasurementsZd z_meas, StatesXd updated_states, Gauss_z z_pred, Gauss_x x_pred) { - StatesZd states; + StatesXd states; states.push_back(x_pred); states.insert(states.end(), updated_states.begin(), updated_states.end()); - Eigen::VectorXd weights = get_weights(z_meas, updated_states, z_pred); + Eigen::VectorXd weights = get_weights(z_meas, z_pred); GaussMixZd gaussian_mixture(weights, states); @@ -89,17 +83,20 @@ class PDAF { } // Getting association probabilities according to textbook p. 123 "Corollary 7.3.3" - Eigen::VectorXd get_weights(MeasurementsZd z_meas, StatesZd updated_states, Gauss_z z_pred) + Eigen::VectorXd get_weights(MeasurementsZd z_meas, Gauss_z z_pred) { Eigen::VectorXd weights(z_meas.size() + 1); + // in case no measurement assosiates with the target double no_association = clutter_intensity_ * (1 - prob_of_detection_); weights(0) = no_association; + // measurements associating with the target for (size_t k = 1; k < z_meas.size() + 1; k++) { - weights(k) = (prob_of_detection_ * z_pred.pdf(z_meas.at(k))); + weights(k) = (prob_of_detection_ * z_pred.pdf(z_meas.at(k - 1))); } + // normalize weights weights /= weights.sum(); return weights; diff --git a/vortex-filtering/test/pdaf_test.cpp b/vortex-filtering/test/pdaf_test.cpp index 8236a6ce..5209cd26 100644 --- a/vortex-filtering/test/pdaf_test.cpp +++ b/vortex-filtering/test/pdaf_test.cpp @@ -6,7 +6,7 @@ using SimplePDAF = PDAF, vortex::models::Ide TEST(PDAF, init) { - SimplePDAF pdaf; + SimplePDAF pdaf(0.0, 0.0, 0.0); EXPECT_EQ(pdaf.gate_threshold_, 0.0); EXPECT_EQ(pdaf.prob_of_detection_, 0.0); EXPECT_EQ(pdaf.clutter_intensity_, 0.0); @@ -18,4 +18,89 @@ TEST(PDAF, init_with_params) EXPECT_EQ(pdaf.gate_threshold_, 1.0); EXPECT_EQ(pdaf.prob_of_detection_, 0.9); EXPECT_EQ(pdaf.clutter_intensity_, 0.1); +} + + +// testing the get_weights function +TEST(PDAF, get_weights_is_calculating) +{ + SimplePDAF pdaf(2.0, 0.8, 1.0); + + vortex::prob::MultiVarGauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); + std::vector meas = {{0.0, 0.0}, {2.0, 1.0}}; + + Eigen::VectorXd weights = pdaf.get_weights(meas, z_pred); + + std::cout << "weights: " << weights << std::endl; + + EXPECT_EQ(weights.size(), 3); +} + +TEST(PDAF, if_no_clutter_first_weight_is_zero) +{ + SimplePDAF pdaf(2.0, 0.8, 0.0); + + vortex::prob::MultiVarGauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); + std::vector meas = {{0.0, 0.0}, {2.0, 1.0}}; + + Eigen::VectorXd weights = pdaf.get_weights(meas, z_pred); + + std::cout << "weights: " << weights << std::endl; + + EXPECT_EQ(weights(0), 0.0); +} + +TEST(PDAF, weights_are_decreasing_with_distance) +{ + SimplePDAF pdaf(2.0, 0.8, 1.0); + + vortex::prob::MultiVarGauss2d z_pred(Eigen::Vector2d(1.0, 1.0), Eigen::Matrix2d::Identity()); + std::vector meas = {{2.0, 1.0}, {3.0, 1.0}, {4.0, 1.0}}; + + Eigen::VectorXd weights = pdaf.get_weights(meas, z_pred); + + std::cout << "weights: " << weights << std::endl; + + EXPECT_GT(weights(1), weights(2)); + EXPECT_GT(weights(2), weights(3)); +} + + +// testing the get_weighted_average function +TEST(PDAF, get_weighted_average_is_calculating) +{ + SimplePDAF pdaf(2.0, 0.8, 1.0); + + vortex::prob::MultiVarGauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); + vortex::prob::MultiVarGauss4d x_pred(Eigen::Vector4d(0.0, 0.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); + std::vector meas = {{0.0, 0.0}, {2.0, 1.0}}; + std::vector updated_states = { + vortex::prob::MultiVarGauss4d(Eigen::Vector4d(0.0, 0.0, 0.0, 0.0), Eigen::Matrix4d::Identity()), + vortex::prob::MultiVarGauss4d(Eigen::Vector4d(1.0, 1.0, 1.0, 1.0), Eigen::Matrix4d::Identity()) + }; + + vortex::prob::MultiVarGauss4d weighted_average = pdaf.get_weighted_average(meas, updated_states, z_pred, x_pred); + + std::cout << "weighted average: " << weighted_average.mean() << std::endl; +} + +TEST(PDAF, average_state_is_in_between_prediction_and_measurement_y_axis) +{ + SimplePDAF pdaf(2.0, 0.8, 1.0); + + vortex::prob::MultiVarGauss2d z_pred(Eigen::Vector2d(1.0, 1.0), Eigen::Matrix2d::Identity()); + vortex::prob::MultiVarGauss4d x_pred(Eigen::Vector4d(1.0, 1.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); + std::vector meas = {{1.0, 2.0}}; + std::vector updated_states = { + vortex::prob::MultiVarGauss4d(Eigen::Vector4d(1.0, 1.5, 0.0, 0.0), Eigen::Matrix4d::Identity()) + }; + + vortex::prob::MultiVarGauss4d weighted_average = pdaf.get_weighted_average(meas, updated_states, z_pred, x_pred); + + EXPECT_GT(weighted_average.mean()(1), x_pred.mean()(1)); + EXPECT_GT(weighted_average.mean()(1), z_pred.mean()(1)); + EXPECT_LT(weighted_average.mean()(1), meas[0](1)); + EXPECT_LT(weighted_average.mean()(1), updated_states[0].mean()(1)); + + std::cout << "weighted average: " << weighted_average.mean() << std::endl; } \ No newline at end of file From 9c37c749f61d98faab0e33f7fb24c0491a1ec445 Mon Sep 17 00:00:00 2001 From: TristanWolfram Date: Fri, 12 Jan 2024 16:12:28 +0100 Subject: [PATCH 03/31] Added axis tests for PDAF --- .../include/vortex_filtering/filters/pdaf.hpp | 16 +++++++++---- vortex-filtering/test/pdaf_test.cpp | 23 ++++++++++++++++++- 2 files changed, 33 insertions(+), 6 deletions(-) diff --git a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp index a17e84f0..7766adcf 100644 --- a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp @@ -5,6 +5,9 @@ #include #include +namespace vortex { +namespace filter { + using std::string; // using std::endl; @@ -35,7 +38,7 @@ class PDAF { std::cout << "Created PDAF class with given models!" << std::endl; } - std::tuple predict_next_state(Gauss_x x_est, MeasurementsZd z_meas, double timestep, DynModPtr dyn_model, SensModPtr sen_model) + std::tuple predict_next_state(const Gauss_x& x_est, const MeasurementsZd& z_meas, double timestep, const DynModPtr& dyn_model, const SensModPtr& sen_model) const { EKF ekf; auto [x_pred, z_pred] = ekf.predict(dyn_model, sen_model, timestep, x_est); @@ -50,7 +53,7 @@ class PDAF { return {predicted_state, inside, outside}; } - std::tuple apply_gate(double gate_threshold, MeasurementsZd z_meas, Gauss_z z_pred) + std::tuple apply_gate(double gate_threshold, const MeasurementsZd& z_meas, const Gauss_z& z_pred) const { MeasurementsZd inside_meas; MeasurementsZd outside_meas; @@ -69,7 +72,7 @@ class PDAF { } // Getting weighted average of the predicted states - Gauss_x get_weighted_average(MeasurementsZd z_meas, StatesXd updated_states, Gauss_z z_pred, Gauss_x x_pred) + Gauss_x get_weighted_average(const MeasurementsZd& z_meas, const StatesXd& updated_states, const Gauss_z& z_pred, const Gauss_x& x_pred) const { StatesXd states; states.push_back(x_pred); @@ -83,7 +86,7 @@ class PDAF { } // Getting association probabilities according to textbook p. 123 "Corollary 7.3.3" - Eigen::VectorXd get_weights(MeasurementsZd z_meas, Gauss_z z_pred) + Eigen::VectorXd get_weights(const MeasurementsZd& z_meas, const Gauss_z& z_pred) const { Eigen::VectorXd weights(z_meas.size() + 1); @@ -132,4 +135,7 @@ class PDAF { { return clutter_intensity_; } -}; \ No newline at end of file +}; + +} // namespace filter +} // namespace vortex \ No newline at end of file diff --git a/vortex-filtering/test/pdaf_test.cpp b/vortex-filtering/test/pdaf_test.cpp index 5209cd26..e2aacf75 100644 --- a/vortex-filtering/test/pdaf_test.cpp +++ b/vortex-filtering/test/pdaf_test.cpp @@ -2,7 +2,7 @@ #include #include -using SimplePDAF = PDAF, vortex::models::IdentitySensorModel<4, 2>>; +using SimplePDAF = vortex::filter::PDAF, vortex::models::IdentitySensorModel<4, 2>>; TEST(PDAF, init) { @@ -102,5 +102,26 @@ TEST(PDAF, average_state_is_in_between_prediction_and_measurement_y_axis) EXPECT_LT(weighted_average.mean()(1), meas[0](1)); EXPECT_LT(weighted_average.mean()(1), updated_states[0].mean()(1)); + std::cout << "weighted average: " << weighted_average.mean() << std::endl; +} + +TEST(PDAF, average_state_is_in_between_prediction_and_measurement_x_axis) +{ + SimplePDAF pdaf(2.0, 0.8, 1.0); + + vortex::prob::MultiVarGauss2d z_pred(Eigen::Vector2d(1.0, 1.0), Eigen::Matrix2d::Identity()); + vortex::prob::MultiVarGauss4d x_pred(Eigen::Vector4d(1.0, 1.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); + std::vector meas = {{2.0, 1.0}}; + std::vector updated_states = { + vortex::prob::MultiVarGauss4d(Eigen::Vector4d(1.5, 1.0, 0.0, 0.0), Eigen::Matrix4d::Identity()) + }; + + vortex::prob::MultiVarGauss4d weighted_average = pdaf.get_weighted_average(meas, updated_states, z_pred, x_pred); + + EXPECT_GT(weighted_average.mean()(0), x_pred.mean()(0)); + EXPECT_GT(weighted_average.mean()(0), z_pred.mean()(0)); + EXPECT_LT(weighted_average.mean()(0), meas[0](0)); + EXPECT_LT(weighted_average.mean()(0), updated_states[0].mean()(0)); + std::cout << "weighted average: " << weighted_average.mean() << std::endl; } \ No newline at end of file From 09d6144d76bb8af3298ae4b66a1e97777d83492c Mon Sep 17 00:00:00 2001 From: TristanWolfram Date: Tue, 16 Jan 2024 19:46:27 +0100 Subject: [PATCH 04/31] finished basic testing! --- .../include/vortex_filtering/filters/pdaf.hpp | 22 +-- vortex-filtering/test/pdaf_test.cpp | 132 +++++++++++++++--- 2 files changed, 125 insertions(+), 29 deletions(-) diff --git a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp index 7766adcf..8f3d971c 100644 --- a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp @@ -15,10 +15,10 @@ template class PDAF { public: using SensModI = typename SensModT::SensModI; - using DynModI = typename DynModT::BaseI; + using DynModI = typename DynModT::DynModI; using DynModPtr = std::shared_ptr; using SensModPtr = std::shared_ptr; - using EKF = vortex::filter::EKF_M; + using EKF = vortex::filter::EKF; using Gauss_z = typename SensModI::Gauss_z; using Gauss_x = typename DynModI::Gauss_x; using Vec_z = typename SensModI::Vec_z; @@ -38,30 +38,32 @@ class PDAF { std::cout << "Created PDAF class with given models!" << std::endl; } - std::tuple predict_next_state(const Gauss_x& x_est, const MeasurementsZd& z_meas, double timestep, const DynModPtr& dyn_model, const SensModPtr& sen_model) const + std::tuple predict_next_state(const Gauss_x& x_est, const MeasurementsZd& z_meas, double timestep, const DynModPtr& dyn_model, const SensModPtr& sen_model) const { - EKF ekf; - auto [x_pred, z_pred] = ekf.predict(dyn_model, sen_model, timestep, x_est); - auto [inside, outside] = apply_gate(gate_threshold_, z_meas, z_pred); + auto [x_pred, z_pred] = EKF::predict(dyn_model, sen_model, timestep, x_est); + auto [inside, outside] = apply_gate(z_meas, z_pred); StatesXd updated; for (const auto& measurement : inside) { - updated.push_back(ekf.update(x_pred, z_pred, measurement)); + updated.push_back(EKF::update(sen_model, x_pred, z_pred, measurement)); } - Gauss_z predicted_state = get_weighted_average(z_meas, updated, z_pred, x_pred); + Gauss_x predicted_state = get_weighted_average(z_meas, updated, z_pred, x_pred); return {predicted_state, inside, outside}; } - std::tuple apply_gate(double gate_threshold, const MeasurementsZd& z_meas, const Gauss_z& z_pred) const + std::tuple apply_gate(const MeasurementsZd& z_meas, const Gauss_z& z_pred) const { MeasurementsZd inside_meas; MeasurementsZd outside_meas; for (const auto& measurement : z_meas) { double distance = z_pred.mahalanobis_distance(measurement); + // std::cout << "measurement: " << measurement << std::endl; + // std::cout << "z_pred: " << z_pred.mean() << std::endl; + // std::cout << "distance: " << distance << std::endl; - if (distance < gate_threshold) { + if (distance < gate_threshold_) { inside_meas.push_back(measurement); } else { outside_meas.push_back(measurement); diff --git a/vortex-filtering/test/pdaf_test.cpp b/vortex-filtering/test/pdaf_test.cpp index e2aacf75..e5c1ea98 100644 --- a/vortex-filtering/test/pdaf_test.cpp +++ b/vortex-filtering/test/pdaf_test.cpp @@ -1,6 +1,7 @@ #include #include #include +#include using SimplePDAF = vortex::filter::PDAF, vortex::models::IdentitySensorModel<4, 2>>; @@ -26,7 +27,7 @@ TEST(PDAF, get_weights_is_calculating) { SimplePDAF pdaf(2.0, 0.8, 1.0); - vortex::prob::MultiVarGauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); + vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); std::vector meas = {{0.0, 0.0}, {2.0, 1.0}}; Eigen::VectorXd weights = pdaf.get_weights(meas, z_pred); @@ -40,7 +41,7 @@ TEST(PDAF, if_no_clutter_first_weight_is_zero) { SimplePDAF pdaf(2.0, 0.8, 0.0); - vortex::prob::MultiVarGauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); + vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); std::vector meas = {{0.0, 0.0}, {2.0, 1.0}}; Eigen::VectorXd weights = pdaf.get_weights(meas, z_pred); @@ -54,7 +55,7 @@ TEST(PDAF, weights_are_decreasing_with_distance) { SimplePDAF pdaf(2.0, 0.8, 1.0); - vortex::prob::MultiVarGauss2d z_pred(Eigen::Vector2d(1.0, 1.0), Eigen::Matrix2d::Identity()); + vortex::prob::Gauss2d z_pred(Eigen::Vector2d(1.0, 1.0), Eigen::Matrix2d::Identity()); std::vector meas = {{2.0, 1.0}, {3.0, 1.0}, {4.0, 1.0}}; Eigen::VectorXd weights = pdaf.get_weights(meas, z_pred); @@ -71,15 +72,15 @@ TEST(PDAF, get_weighted_average_is_calculating) { SimplePDAF pdaf(2.0, 0.8, 1.0); - vortex::prob::MultiVarGauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); - vortex::prob::MultiVarGauss4d x_pred(Eigen::Vector4d(0.0, 0.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); + vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); + vortex::prob::Gauss4d x_pred(Eigen::Vector4d(0.0, 0.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); std::vector meas = {{0.0, 0.0}, {2.0, 1.0}}; - std::vector updated_states = { - vortex::prob::MultiVarGauss4d(Eigen::Vector4d(0.0, 0.0, 0.0, 0.0), Eigen::Matrix4d::Identity()), - vortex::prob::MultiVarGauss4d(Eigen::Vector4d(1.0, 1.0, 1.0, 1.0), Eigen::Matrix4d::Identity()) + std::vector updated_states = { + vortex::prob::Gauss4d(Eigen::Vector4d(0.0, 0.0, 0.0, 0.0), Eigen::Matrix4d::Identity()), + vortex::prob::Gauss4d(Eigen::Vector4d(1.0, 1.0, 1.0, 1.0), Eigen::Matrix4d::Identity()) }; - vortex::prob::MultiVarGauss4d weighted_average = pdaf.get_weighted_average(meas, updated_states, z_pred, x_pred); + vortex::prob::Gauss4d weighted_average = pdaf.get_weighted_average(meas, updated_states, z_pred, x_pred); std::cout << "weighted average: " << weighted_average.mean() << std::endl; } @@ -88,14 +89,14 @@ TEST(PDAF, average_state_is_in_between_prediction_and_measurement_y_axis) { SimplePDAF pdaf(2.0, 0.8, 1.0); - vortex::prob::MultiVarGauss2d z_pred(Eigen::Vector2d(1.0, 1.0), Eigen::Matrix2d::Identity()); - vortex::prob::MultiVarGauss4d x_pred(Eigen::Vector4d(1.0, 1.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); + vortex::prob::Gauss2d z_pred(Eigen::Vector2d(1.0, 1.0), Eigen::Matrix2d::Identity()); + vortex::prob::Gauss4d x_pred(Eigen::Vector4d(1.0, 1.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); std::vector meas = {{1.0, 2.0}}; - std::vector updated_states = { - vortex::prob::MultiVarGauss4d(Eigen::Vector4d(1.0, 1.5, 0.0, 0.0), Eigen::Matrix4d::Identity()) + std::vector updated_states = { + vortex::prob::Gauss4d(Eigen::Vector4d(1.0, 1.5, 0.0, 0.0), Eigen::Matrix4d::Identity()) }; - vortex::prob::MultiVarGauss4d weighted_average = pdaf.get_weighted_average(meas, updated_states, z_pred, x_pred); + vortex::prob::Gauss4d weighted_average = pdaf.get_weighted_average(meas, updated_states, z_pred, x_pred); EXPECT_GT(weighted_average.mean()(1), x_pred.mean()(1)); EXPECT_GT(weighted_average.mean()(1), z_pred.mean()(1)); @@ -109,14 +110,14 @@ TEST(PDAF, average_state_is_in_between_prediction_and_measurement_x_axis) { SimplePDAF pdaf(2.0, 0.8, 1.0); - vortex::prob::MultiVarGauss2d z_pred(Eigen::Vector2d(1.0, 1.0), Eigen::Matrix2d::Identity()); - vortex::prob::MultiVarGauss4d x_pred(Eigen::Vector4d(1.0, 1.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); + vortex::prob::Gauss2d z_pred(Eigen::Vector2d(1.0, 1.0), Eigen::Matrix2d::Identity()); + vortex::prob::Gauss4d x_pred(Eigen::Vector4d(1.0, 1.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); std::vector meas = {{2.0, 1.0}}; - std::vector updated_states = { - vortex::prob::MultiVarGauss4d(Eigen::Vector4d(1.5, 1.0, 0.0, 0.0), Eigen::Matrix4d::Identity()) + std::vector updated_states = { + vortex::prob::Gauss4d(Eigen::Vector4d(1.5, 1.0, 0.0, 0.0), Eigen::Matrix4d::Identity()) }; - vortex::prob::MultiVarGauss4d weighted_average = pdaf.get_weighted_average(meas, updated_states, z_pred, x_pred); + vortex::prob::Gauss4d weighted_average = pdaf.get_weighted_average(meas, updated_states, z_pred, x_pred); EXPECT_GT(weighted_average.mean()(0), x_pred.mean()(0)); EXPECT_GT(weighted_average.mean()(0), z_pred.mean()(0)); @@ -124,4 +125,97 @@ TEST(PDAF, average_state_is_in_between_prediction_and_measurement_x_axis) EXPECT_LT(weighted_average.mean()(0), updated_states[0].mean()(0)); std::cout << "weighted average: " << weighted_average.mean() << std::endl; +} + +TEST(PDAF, average_state_is_in_between_prediction_and_measurement_both_axes) +{ + SimplePDAF pdaf(2.0, 0.8, 1.0); + + vortex::prob::Gauss2d z_pred(Eigen::Vector2d(1.0, 1.0), Eigen::Matrix2d::Identity()); + vortex::prob::Gauss4d x_pred(Eigen::Vector4d(1.0, 1.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); + std::vector meas = {{2.0, 2.0}}; + std::vector updated_states = { + vortex::prob::Gauss4d(Eigen::Vector4d(1.5, 1.5, 0.0, 0.0), Eigen::Matrix4d::Identity()) + }; + + vortex::prob::Gauss4d weighted_average = pdaf.get_weighted_average(meas, updated_states, z_pred, x_pred); + + EXPECT_GT(weighted_average.mean()(0), x_pred.mean()(0)); + EXPECT_GT(weighted_average.mean()(0), z_pred.mean()(0)); + EXPECT_LT(weighted_average.mean()(0), meas[0](0)); + EXPECT_LT(weighted_average.mean()(0), updated_states[0].mean()(0)); + + EXPECT_GT(weighted_average.mean()(1), x_pred.mean()(1)); + EXPECT_GT(weighted_average.mean()(1), z_pred.mean()(1)); + EXPECT_LT(weighted_average.mean()(1), meas[0](1)); + EXPECT_LT(weighted_average.mean()(1), updated_states[0].mean()(1)); + + std::cout << "weighted average: " << weighted_average.mean() << std::endl; +} + +// testing the apply_gate function +TEST(PDAF, apply_gate_is_calculating) +{ + + double gate_threshold = 1.8; + SimplePDAF pdaf(gate_threshold, 0.8, 1.0); + + vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); + std::vector meas = {{0.0, 1.0}, {1.0, 0.0}, {1.0, 1.0},{0.0, 2.0}, {2.0, 0.0}, {2.0, 2.0}}; + + auto [inside, outside] = pdaf.apply_gate(meas, z_pred); +} + +TEST(PDAF, apply_gate_is_separating_correctly) +{ + double gate_threshold = 1.8; + SimplePDAF pdaf(gate_threshold, 0.8, 1.0); + + vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); + std::vector meas = {{0.0, 1.0}, {1.0, 0.0}, {1.0, 1.0},{0.0, 2.0}, {2.0, 0.0}, {2.0, 2.0}}; + + auto [inside, outside] = pdaf.apply_gate(meas, z_pred); + + EXPECT_EQ(inside.size(), 3u); + EXPECT_EQ(outside.size(), 3u); +} + +TEST(PDAF, apply_gate_is_separating_correctly_2) +{ + double gate_threshold = 2.1; + SimplePDAF pdaf(gate_threshold, 0.8, 1.0); + + vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); + std::vector meas = {{0.0, 1.0}, {1.0, 0.0}, {1.0, 1.0},{0.0, 2.0}, {2.0, 0.0}, {2.0, 2.0}}; + + auto [inside, outside] = pdaf.apply_gate(meas, z_pred); + + EXPECT_EQ(inside.size(), 5u); + EXPECT_EQ(outside.size(), 1u); +} + +// testing the predict_next_state function +TEST(PDAF, predict_next_state_is_calculating) +{ + SimplePDAF pdaf(2.0, 0.8, 1.0); + + vortex::prob::Gauss4d x_est(Eigen::Vector4d(0.0, 0.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); + std::vector meas = {{0.0, 1.0}, {1.0, 0.0}, {1.0, 1.0},{0.0, 2.0}, {2.0, 0.0}, {2.0, 2.0}}; + + auto dyn_model = std::make_shared>(1.0); + auto sen_model = std::make_shared>(1.0); + + auto [x_pred, inside, outside] = pdaf.predict_next_state(x_est, meas, 1.0, dyn_model, sen_model); + std::cout << "x_pred: " << x_pred.mean() << std::endl; + + // Gnuplot gp; + // gp << "set xrange [-5:5]\nset yrange [-5:5]\n"; + // gp << "set style circle radius 0.05\n"; + // gp << "plot '-' with circles title 'Samples' fs transparent solid 0.05 noborder\n"; + // gp.send1d(meas); + + // gp << "set object 1 ellipse center " << true_mean(0) << "," << true_mean(1) << " size " << majorAxisLength << "," << minorAxisLength << " angle " << angle + // << "fs empty border lc rgb 'cyan'\n"; + // gp << "replot\n"; + } \ No newline at end of file From 164400322fd45521a87e00c1af63593f32810c81 Mon Sep 17 00:00:00 2001 From: TristanWolfram Date: Sun, 21 Jan 2024 18:32:59 +0100 Subject: [PATCH 05/31] addded some more visualization --- .../include/vortex_filtering/filters/pdaf.hpp | 18 ++--- vortex-filtering/test/pdaf_test.cpp | 76 +++++++++++++++---- 2 files changed, 69 insertions(+), 25 deletions(-) diff --git a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp index 8f3d971c..753d3d71 100644 --- a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp @@ -38,18 +38,18 @@ class PDAF { std::cout << "Created PDAF class with given models!" << std::endl; } - std::tuple predict_next_state(const Gauss_x& x_est, const MeasurementsZd& z_meas, double timestep, const DynModPtr& dyn_model, const SensModPtr& sen_model) const + std::tuple predict_next_state(const Gauss_x& x_est, const MeasurementsZd& z_meas, double timestep, const DynModPtr& dyn_model, const SensModPtr& sen_model) const { auto [x_pred, z_pred] = EKF::predict(dyn_model, sen_model, timestep, x_est); auto [inside, outside] = apply_gate(z_meas, z_pred); - StatesXd updated; + StatesXd x_updated; for (const auto& measurement : inside) { - updated.push_back(EKF::update(sen_model, x_pred, z_pred, measurement)); + x_updated.push_back(EKF::update(sen_model, x_pred, z_pred, measurement)); } - Gauss_x predicted_state = get_weighted_average(z_meas, updated, z_pred, x_pred); - return {predicted_state, inside, outside}; + Gauss_x x_final = get_weighted_average(z_meas, x_updated, z_pred, x_pred); + return {x_final, inside, outside, x_pred, z_pred, x_updated}; } std::tuple apply_gate(const MeasurementsZd& z_meas, const Gauss_z& z_pred) const @@ -59,11 +59,11 @@ class PDAF { for (const auto& measurement : z_meas) { double distance = z_pred.mahalanobis_distance(measurement); - // std::cout << "measurement: " << measurement << std::endl; - // std::cout << "z_pred: " << z_pred.mean() << std::endl; - // std::cout << "distance: " << distance << std::endl; + std::cout << "measurement: " << measurement << std::endl; + std::cout << "z_pred: " << z_pred.mean() << std::endl; + std::cout << "distance: " << distance << std::endl; - if (distance < gate_threshold_) { + if (distance <= gate_threshold_) { inside_meas.push_back(measurement); } else { outside_meas.push_back(measurement); diff --git a/vortex-filtering/test/pdaf_test.cpp b/vortex-filtering/test/pdaf_test.cpp index e5c1ea98..af639f76 100644 --- a/vortex-filtering/test/pdaf_test.cpp +++ b/vortex-filtering/test/pdaf_test.cpp @@ -1,5 +1,6 @@ #include #include +#include #include #include @@ -168,16 +169,40 @@ TEST(PDAF, apply_gate_is_calculating) TEST(PDAF, apply_gate_is_separating_correctly) { - double gate_threshold = 1.8; + double gate_threshold = 2; SimplePDAF pdaf(gate_threshold, 0.8, 1.0); + + Eigen::Matrix2d cov; + cov << 1.0, 0.0, + 0.0, 4.0; - vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); - std::vector meas = {{0.0, 1.0}, {1.0, 0.0}, {1.0, 1.0},{0.0, 2.0}, {2.0, 0.0}, {2.0, 2.0}}; + vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), cov); + std::vector meas = {{0.0, 4.0}, {4.0, 0.0}}; auto [inside, outside] = pdaf.apply_gate(meas, z_pred); - EXPECT_EQ(inside.size(), 3u); - EXPECT_EQ(outside.size(), 3u); + EXPECT_EQ(inside.size(), 1u); + EXPECT_EQ(outside.size(), 1u); + EXPECT_EQ(inside[0], meas[0]); + EXPECT_EQ(outside[0], meas[1]); + + Gnuplot gp; + gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; + gp << "set style circle radius 0.05\n"; + gp << "plot '-' with circles title 'Samples' fs transparent solid 1 noborder\n"; + gp.send1d(meas); + + int object_counter = 0; + + gp << "set object " << ++object_counter << " circle center " << z_pred.mean()(0) << "," << z_pred.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; + gp << "replot\n"; + + vortex::plotting::Ellipse prediction = vortex::plotting::gauss_to_ellipse(z_pred, 5.991); + + gp << "set object " << ++object_counter << " ellipse center " << prediction.x << "," << prediction.y << " size " << prediction.a << "," << prediction.b << " angle " << prediction.angle + << "fs empty border lc rgb 'cyan'\n"; + gp << "replot\n"; + } TEST(PDAF, apply_gate_is_separating_correctly_2) @@ -197,7 +222,9 @@ TEST(PDAF, apply_gate_is_separating_correctly_2) // testing the predict_next_state function TEST(PDAF, predict_next_state_is_calculating) { - SimplePDAF pdaf(2.0, 0.8, 1.0); + + double gate_threshold = 1.0; + SimplePDAF pdaf(gate_threshold, 0.8, 1.0); vortex::prob::Gauss4d x_est(Eigen::Vector4d(0.0, 0.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); std::vector meas = {{0.0, 1.0}, {1.0, 0.0}, {1.0, 1.0},{0.0, 2.0}, {2.0, 0.0}, {2.0, 2.0}}; @@ -205,17 +232,34 @@ TEST(PDAF, predict_next_state_is_calculating) auto dyn_model = std::make_shared>(1.0); auto sen_model = std::make_shared>(1.0); - auto [x_pred, inside, outside] = pdaf.predict_next_state(x_est, meas, 1.0, dyn_model, sen_model); - std::cout << "x_pred: " << x_pred.mean() << std::endl; + auto [x_final, inside, outside, x_pred, z_pred, x_updated] = pdaf.predict_next_state(x_est, meas, 1.0, dyn_model, sen_model); + std::cout << "x_final: " << x_final.mean() << std::endl; + + Gnuplot gp; + gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; + gp << "set style circle radius 0.05\n"; + gp << "plot '-' with circles title 'Samples' fs transparent solid 1 noborder\n"; + gp.send1d(meas); + + int object_counter = 0; - // Gnuplot gp; - // gp << "set xrange [-5:5]\nset yrange [-5:5]\n"; - // gp << "set style circle radius 0.05\n"; - // gp << "plot '-' with circles title 'Samples' fs transparent solid 0.05 noborder\n"; - // gp.send1d(meas); + gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 << "fs empty border lc rgb 'black'\n"; + gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; + gp << "replot\n"; - // gp << "set object 1 ellipse center " << true_mean(0) << "," << true_mean(1) << " size " << majorAxisLength << "," << minorAxisLength << " angle " << angle - // << "fs empty border lc rgb 'cyan'\n"; - // gp << "replot\n"; + vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); + + gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle + << "fs empty border lc rgb 'cyan'\n"; + gp << "replot\n"; + + for (const auto& state: x_updated) + { + vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2,2)); + vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); + + gp << "set object " << ++object_counter << " ellipse center " << ellipse.x << "," << ellipse.y << " size " << ellipse.a << "," << ellipse.b << " angle " << ellipse.angle << "fs empty border lc rgb 'blue'\n"; + }; + gp << "replot\n"; } \ No newline at end of file From 75be4f69d08e253b07b88d14f2d853c29d6bce33 Mon Sep 17 00:00:00 2001 From: TristanWolfram Date: Sun, 28 Jan 2024 12:46:27 +0100 Subject: [PATCH 06/31] =?UTF-8?q?=F0=9F=90=9B=20Refactor=20PDAF=20class=20?= =?UTF-8?q?to=20be=20without=20parameters?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/vortex_filtering/filters/pdaf.hpp | 93 ++++---- vortex-filtering/test/pdaf_test.cpp | 210 ++++++++++++++---- 2 files changed, 214 insertions(+), 89 deletions(-) diff --git a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp index 753d3d71..a17ffd79 100644 --- a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp @@ -26,33 +26,45 @@ class PDAF { using StatesXd = std::vector; using GaussMixZd = vortex::prob::GaussianMixture; - double gate_threshold_; - double prob_of_detection_; - double clutter_intensity_; - - PDAF(double gate, double prob_of_detection, double clutter_intensity) - : gate_threshold_(gate) - , prob_of_detection_(prob_of_detection) - , clutter_intensity_(clutter_intensity) + PDAF() { - std::cout << "Created PDAF class with given models!" << std::endl; + std::cout << "Created PDAF class!" << std::endl; } - std::tuple predict_next_state(const Gauss_x& x_est, const MeasurementsZd& z_meas, double timestep, const DynModPtr& dyn_model, const SensModPtr& sen_model) const + std::tuple predict_next_state( + const Gauss_x& x_est, + const MeasurementsZd& z_meas, + double timestep, + const DynModPtr& dyn_model, + const SensModPtr& sen_model, + double gate_threshold, + double prob_of_detection, + double clutter_intensity + ) const { auto [x_pred, z_pred] = EKF::predict(dyn_model, sen_model, timestep, x_est); - auto [inside, outside] = apply_gate(z_meas, z_pred); + auto [inside, outside] = apply_gate(z_meas, z_pred, gate_threshold); StatesXd x_updated; for (const auto& measurement : inside) { x_updated.push_back(EKF::update(sen_model, x_pred, z_pred, measurement)); } - Gauss_x x_final = get_weighted_average(z_meas, x_updated, z_pred, x_pred); + Gauss_x x_final = get_weighted_average( + z_meas, + x_updated, + z_pred, + x_pred, + prob_of_detection, + clutter_intensity); return {x_final, inside, outside, x_pred, z_pred, x_updated}; } - std::tuple apply_gate(const MeasurementsZd& z_meas, const Gauss_z& z_pred) const + std::tuple apply_gate( + const MeasurementsZd& z_meas, + const Gauss_z& z_pred, + double gate_threshold + ) const { MeasurementsZd inside_meas; MeasurementsZd outside_meas; @@ -63,7 +75,7 @@ class PDAF { std::cout << "z_pred: " << z_pred.mean() << std::endl; std::cout << "distance: " << distance << std::endl; - if (distance <= gate_threshold_) { + if (distance <= gate_threshold) { inside_meas.push_back(measurement); } else { outside_meas.push_back(measurement); @@ -74,13 +86,20 @@ class PDAF { } // Getting weighted average of the predicted states - Gauss_x get_weighted_average(const MeasurementsZd& z_meas, const StatesXd& updated_states, const Gauss_z& z_pred, const Gauss_x& x_pred) const + Gauss_x get_weighted_average( + const MeasurementsZd& z_meas, + const StatesXd& updated_states, + const Gauss_z& z_pred, + const Gauss_x& x_pred, + double prob_of_detection, + double clutter_intensity + ) const { StatesXd states; states.push_back(x_pred); states.insert(states.end(), updated_states.begin(), updated_states.end()); - Eigen::VectorXd weights = get_weights(z_meas, z_pred); + Eigen::VectorXd weights = get_weights(z_meas, z_pred, prob_of_detection, clutter_intensity); GaussMixZd gaussian_mixture(weights, states); @@ -88,17 +107,22 @@ class PDAF { } // Getting association probabilities according to textbook p. 123 "Corollary 7.3.3" - Eigen::VectorXd get_weights(const MeasurementsZd& z_meas, const Gauss_z& z_pred) const + Eigen::VectorXd get_weights( + const MeasurementsZd& z_meas, + const Gauss_z& z_pred, + double prob_of_detection, + double clutter_intensity + ) const { Eigen::VectorXd weights(z_meas.size() + 1); // in case no measurement assosiates with the target - double no_association = clutter_intensity_ * (1 - prob_of_detection_); + double no_association = clutter_intensity * (1 - prob_of_detection); weights(0) = no_association; // measurements associating with the target for (size_t k = 1; k < z_meas.size() + 1; k++) { - weights(k) = (prob_of_detection_ * z_pred.pdf(z_meas.at(k - 1))); + weights(k) = (prob_of_detection * z_pred.pdf(z_meas.at(k - 1))); } // normalize weights @@ -106,37 +130,6 @@ class PDAF { return weights; } - - // Getter and setter for mem - void set_gate_threshold(double gate_threshold) - { - gate_threshold_ = gate_threshold; - } - - void set_prob_of_detection(double prob_of_detection) - { - prob_of_detection_ = prob_of_detection; - } - - void set_clutter_intensity(double clutter_intensity) - { - clutter_intensity_ = clutter_intensity; - } - - double get_gate_threshold() - { - return gate_threshold_; - } - - double get_prob_of_detection() - { - return prob_of_detection_; - } - - double get_clutter_intensity() - { - return clutter_intensity_; - } }; } // namespace filter diff --git a/vortex-filtering/test/pdaf_test.cpp b/vortex-filtering/test/pdaf_test.cpp index af639f76..2ef57e1d 100644 --- a/vortex-filtering/test/pdaf_test.cpp +++ b/vortex-filtering/test/pdaf_test.cpp @@ -8,30 +8,22 @@ using SimplePDAF = vortex::filter::PDAF, vor TEST(PDAF, init) { - SimplePDAF pdaf(0.0, 0.0, 0.0); - EXPECT_EQ(pdaf.gate_threshold_, 0.0); - EXPECT_EQ(pdaf.prob_of_detection_, 0.0); - EXPECT_EQ(pdaf.clutter_intensity_, 0.0); -} - -TEST(PDAF, init_with_params) -{ - SimplePDAF pdaf(1.0, 0.9, 0.1); - EXPECT_EQ(pdaf.gate_threshold_, 1.0); - EXPECT_EQ(pdaf.prob_of_detection_, 0.9); - EXPECT_EQ(pdaf.clutter_intensity_, 0.1); + SimplePDAF pdaf; } // testing the get_weights function TEST(PDAF, get_weights_is_calculating) { - SimplePDAF pdaf(2.0, 0.8, 1.0); + SimplePDAF pdaf; + + double prob_of_detection = 0.8; + double clutter_intensity = 1.0; vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); std::vector meas = {{0.0, 0.0}, {2.0, 1.0}}; - Eigen::VectorXd weights = pdaf.get_weights(meas, z_pred); + Eigen::VectorXd weights = pdaf.get_weights(meas, z_pred, prob_of_detection, clutter_intensity); std::cout << "weights: " << weights << std::endl; @@ -40,12 +32,15 @@ TEST(PDAF, get_weights_is_calculating) TEST(PDAF, if_no_clutter_first_weight_is_zero) { - SimplePDAF pdaf(2.0, 0.8, 0.0); + SimplePDAF pdaf; + + double prob_of_detection = 0.8; + double clutter_intensity = 0.0; vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); std::vector meas = {{0.0, 0.0}, {2.0, 1.0}}; - Eigen::VectorXd weights = pdaf.get_weights(meas, z_pred); + Eigen::VectorXd weights = pdaf.get_weights(meas, z_pred, prob_of_detection, clutter_intensity); std::cout << "weights: " << weights << std::endl; @@ -54,12 +49,15 @@ TEST(PDAF, if_no_clutter_first_weight_is_zero) TEST(PDAF, weights_are_decreasing_with_distance) { - SimplePDAF pdaf(2.0, 0.8, 1.0); + SimplePDAF pdaf; + + double prob_of_detection = 0.8; + double clutter_intensity = 1.0; vortex::prob::Gauss2d z_pred(Eigen::Vector2d(1.0, 1.0), Eigen::Matrix2d::Identity()); std::vector meas = {{2.0, 1.0}, {3.0, 1.0}, {4.0, 1.0}}; - Eigen::VectorXd weights = pdaf.get_weights(meas, z_pred); + Eigen::VectorXd weights = pdaf.get_weights(meas, z_pred, prob_of_detection, clutter_intensity); std::cout << "weights: " << weights << std::endl; @@ -71,7 +69,10 @@ TEST(PDAF, weights_are_decreasing_with_distance) // testing the get_weighted_average function TEST(PDAF, get_weighted_average_is_calculating) { - SimplePDAF pdaf(2.0, 0.8, 1.0); + SimplePDAF pdaf; + + double prob_of_detection = 0.8; + double clutter_intensity = 1.0; vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); vortex::prob::Gauss4d x_pred(Eigen::Vector4d(0.0, 0.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); @@ -81,14 +82,23 @@ TEST(PDAF, get_weighted_average_is_calculating) vortex::prob::Gauss4d(Eigen::Vector4d(1.0, 1.0, 1.0, 1.0), Eigen::Matrix4d::Identity()) }; - vortex::prob::Gauss4d weighted_average = pdaf.get_weighted_average(meas, updated_states, z_pred, x_pred); + vortex::prob::Gauss4d weighted_average = pdaf.get_weighted_average( + meas, + updated_states, + z_pred, x_pred, + prob_of_detection, + clutter_intensity + ); std::cout << "weighted average: " << weighted_average.mean() << std::endl; } TEST(PDAF, average_state_is_in_between_prediction_and_measurement_y_axis) { - SimplePDAF pdaf(2.0, 0.8, 1.0); + SimplePDAF pdaf; + + double prob_of_detection = 0.8; + double clutter_intensity = 1.0; vortex::prob::Gauss2d z_pred(Eigen::Vector2d(1.0, 1.0), Eigen::Matrix2d::Identity()); vortex::prob::Gauss4d x_pred(Eigen::Vector4d(1.0, 1.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); @@ -97,7 +107,14 @@ TEST(PDAF, average_state_is_in_between_prediction_and_measurement_y_axis) vortex::prob::Gauss4d(Eigen::Vector4d(1.0, 1.5, 0.0, 0.0), Eigen::Matrix4d::Identity()) }; - vortex::prob::Gauss4d weighted_average = pdaf.get_weighted_average(meas, updated_states, z_pred, x_pred); + vortex::prob::Gauss4d weighted_average = pdaf.get_weighted_average( + meas, + updated_states, + z_pred, + x_pred, + prob_of_detection, + clutter_intensity + ); EXPECT_GT(weighted_average.mean()(1), x_pred.mean()(1)); EXPECT_GT(weighted_average.mean()(1), z_pred.mean()(1)); @@ -109,7 +126,10 @@ TEST(PDAF, average_state_is_in_between_prediction_and_measurement_y_axis) TEST(PDAF, average_state_is_in_between_prediction_and_measurement_x_axis) { - SimplePDAF pdaf(2.0, 0.8, 1.0); + SimplePDAF pdaf; + + double prob_of_detection = 0.8; + double clutter_intensity = 1.0; vortex::prob::Gauss2d z_pred(Eigen::Vector2d(1.0, 1.0), Eigen::Matrix2d::Identity()); vortex::prob::Gauss4d x_pred(Eigen::Vector4d(1.0, 1.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); @@ -118,7 +138,14 @@ TEST(PDAF, average_state_is_in_between_prediction_and_measurement_x_axis) vortex::prob::Gauss4d(Eigen::Vector4d(1.5, 1.0, 0.0, 0.0), Eigen::Matrix4d::Identity()) }; - vortex::prob::Gauss4d weighted_average = pdaf.get_weighted_average(meas, updated_states, z_pred, x_pred); + vortex::prob::Gauss4d weighted_average = pdaf.get_weighted_average( + meas, + updated_states, + z_pred, + x_pred, + prob_of_detection, + clutter_intensity + ); EXPECT_GT(weighted_average.mean()(0), x_pred.mean()(0)); EXPECT_GT(weighted_average.mean()(0), z_pred.mean()(0)); @@ -130,7 +157,10 @@ TEST(PDAF, average_state_is_in_between_prediction_and_measurement_x_axis) TEST(PDAF, average_state_is_in_between_prediction_and_measurement_both_axes) { - SimplePDAF pdaf(2.0, 0.8, 1.0); + SimplePDAF pdaf; + + double prob_of_detection = 0.8; + double clutter_intensity = 1.0; vortex::prob::Gauss2d z_pred(Eigen::Vector2d(1.0, 1.0), Eigen::Matrix2d::Identity()); vortex::prob::Gauss4d x_pred(Eigen::Vector4d(1.0, 1.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); @@ -139,7 +169,14 @@ TEST(PDAF, average_state_is_in_between_prediction_and_measurement_both_axes) vortex::prob::Gauss4d(Eigen::Vector4d(1.5, 1.5, 0.0, 0.0), Eigen::Matrix4d::Identity()) }; - vortex::prob::Gauss4d weighted_average = pdaf.get_weighted_average(meas, updated_states, z_pred, x_pred); + vortex::prob::Gauss4d weighted_average = pdaf.get_weighted_average( + meas, + updated_states, + z_pred, + x_pred, + prob_of_detection, + clutter_intensity + ); EXPECT_GT(weighted_average.mean()(0), x_pred.mean()(0)); EXPECT_GT(weighted_average.mean()(0), z_pred.mean()(0)); @@ -158,19 +195,24 @@ TEST(PDAF, average_state_is_in_between_prediction_and_measurement_both_axes) TEST(PDAF, apply_gate_is_calculating) { + + SimplePDAF pdaf; + double gate_threshold = 1.8; - SimplePDAF pdaf(gate_threshold, 0.8, 1.0); vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); std::vector meas = {{0.0, 1.0}, {1.0, 0.0}, {1.0, 1.0},{0.0, 2.0}, {2.0, 0.0}, {2.0, 2.0}}; - auto [inside, outside] = pdaf.apply_gate(meas, z_pred); + auto [inside, outside] = pdaf.apply_gate( + meas, + z_pred, + gate_threshold); } TEST(PDAF, apply_gate_is_separating_correctly) { double gate_threshold = 2; - SimplePDAF pdaf(gate_threshold, 0.8, 1.0); + SimplePDAF pdaf; Eigen::Matrix2d cov; cov << 1.0, 0.0, @@ -179,7 +221,10 @@ TEST(PDAF, apply_gate_is_separating_correctly) vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), cov); std::vector meas = {{0.0, 4.0}, {4.0, 0.0}}; - auto [inside, outside] = pdaf.apply_gate(meas, z_pred); + auto [inside, outside] = pdaf.apply_gate( + meas, + z_pred, + gate_threshold); EXPECT_EQ(inside.size(), 1u); EXPECT_EQ(outside.size(), 1u); @@ -188,8 +233,9 @@ TEST(PDAF, apply_gate_is_separating_correctly) Gnuplot gp; gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; + gp << "set size ratio -1\n"; gp << "set style circle radius 0.05\n"; - gp << "plot '-' with circles title 'Samples' fs transparent solid 1 noborder\n"; + gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; gp.send1d(meas); int object_counter = 0; @@ -197,7 +243,7 @@ TEST(PDAF, apply_gate_is_separating_correctly) gp << "set object " << ++object_counter << " circle center " << z_pred.mean()(0) << "," << z_pred.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; gp << "replot\n"; - vortex::plotting::Ellipse prediction = vortex::plotting::gauss_to_ellipse(z_pred, 5.991); + vortex::plotting::Ellipse prediction = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); gp << "set object " << ++object_counter << " ellipse center " << prediction.x << "," << prediction.y << " size " << prediction.a << "," << prediction.b << " angle " << prediction.angle << "fs empty border lc rgb 'cyan'\n"; @@ -208,12 +254,15 @@ TEST(PDAF, apply_gate_is_separating_correctly) TEST(PDAF, apply_gate_is_separating_correctly_2) { double gate_threshold = 2.1; - SimplePDAF pdaf(gate_threshold, 0.8, 1.0); + SimplePDAF pdaf; vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); std::vector meas = {{0.0, 1.0}, {1.0, 0.0}, {1.0, 1.0},{0.0, 2.0}, {2.0, 0.0}, {2.0, 2.0}}; - auto [inside, outside] = pdaf.apply_gate(meas, z_pred); + auto [inside, outside] = pdaf.apply_gate( + meas, + z_pred, + gate_threshold); EXPECT_EQ(inside.size(), 5u); EXPECT_EQ(outside.size(), 1u); @@ -223,8 +272,10 @@ TEST(PDAF, apply_gate_is_separating_correctly_2) TEST(PDAF, predict_next_state_is_calculating) { - double gate_threshold = 1.0; - SimplePDAF pdaf(gate_threshold, 0.8, 1.0); + double gate_threshold = 1.12; + double prob_of_detection = 0.8; + double clutter_intensity = 1.0; + SimplePDAF pdaf; vortex::prob::Gauss4d x_est(Eigen::Vector4d(0.0, 0.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); std::vector meas = {{0.0, 1.0}, {1.0, 0.0}, {1.0, 1.0},{0.0, 2.0}, {2.0, 0.0}, {2.0, 2.0}}; @@ -232,21 +283,35 @@ TEST(PDAF, predict_next_state_is_calculating) auto dyn_model = std::make_shared>(1.0); auto sen_model = std::make_shared>(1.0); - auto [x_final, inside, outside, x_pred, z_pred, x_updated] = pdaf.predict_next_state(x_est, meas, 1.0, dyn_model, sen_model); + auto [x_final, inside, outside, x_pred, z_pred, x_updated] = pdaf.predict_next_state( + x_est, + meas, + 1.0, + dyn_model, + sen_model, + gate_threshold, + prob_of_detection, + clutter_intensity); std::cout << "x_final: " << x_final.mean() << std::endl; Gnuplot gp; gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; + gp << "set size ratio -1\n"; gp << "set style circle radius 0.05\n"; - gp << "plot '-' with circles title 'Samples' fs transparent solid 1 noborder\n"; + gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; gp.send1d(meas); int object_counter = 0; gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 << "fs empty border lc rgb 'black'\n"; + gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 << "fs empty border lc rgb 'pink'\n"; gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; gp << "replot\n"; + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " nohead lc rgb 'pink'\n"; + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n"; + gp << "replot\n"; + vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle @@ -258,8 +323,75 @@ TEST(PDAF, predict_next_state_is_calculating) vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2,2)); vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); - gp << "set object " << ++object_counter << " ellipse center " << ellipse.x << "," << ellipse.y << " size " << ellipse.a << "," << ellipse.b << " angle " << ellipse.angle << "fs empty border lc rgb 'blue'\n"; + gp << "set object " << ++object_counter + << " ellipse center " << ellipse.x << "," << ellipse.y + << " size " << ellipse.a << "," << ellipse.b + << " angle " << ellipse.angle + << "fs solid 0.2 noborder front lc rgb 'blue'\n"; }; + gp << "replot\n"; +} + +TEST(PDAF, predict_next_state_2) +{ + + double gate_threshold = 2; + double prob_of_detection = 0.8; + double clutter_intensity = 1.0; + SimplePDAF pdaf; + + vortex::prob::Gauss4d x_est(Eigen::Vector4d(1.0, 1.5, -2.0, 0.0), Eigen::Matrix4d::Identity()); + std::vector meas = {{-3.0, -3.0}, {0.0, 0.0}, {-1.2, 1.5},{-2.0, -2.0}, {2.0, 0.0}, {-1.0, 1.0}}; + + auto dyn_model = std::make_shared>(1.0); + auto sen_model = std::make_shared>(0.5); + + auto [x_final, inside, outside, x_pred, z_pred, x_updated] = pdaf.predict_next_state( + x_est, + meas, + 1.0, + dyn_model, + sen_model, + gate_threshold, + prob_of_detection, + clutter_intensity); + std::cout << "x_final: " << x_final.mean() << std::endl; + + Gnuplot gp; + gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; + gp << "set size ratio -1\n"; + gp << "set style circle radius 0.05\n"; + gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; + gp.send1d(meas); + + int object_counter = 0; + + gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 << "fs empty border lc rgb 'black'\n"; + gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 << "fs empty border lc rgb 'pink'\n"; + gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; + gp << "replot\n"; + + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " nohead lc rgb 'pink'\n"; + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n"; gp << "replot\n"; + vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); + + gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle + << "fs empty border lc rgb 'cyan'\n"; + + for (const auto& state: x_updated) + { + vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2,2)); + vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); + + gp << "set object " << ++object_counter + << " ellipse center " << ellipse.x << "," << ellipse.y + << " size " << ellipse.a << "," << ellipse.b + << " angle " << ellipse.angle + << "fs solid 0.2 noborder lc rgb 'blue'\n"; + }; + + gp << "replot\n"; + } \ No newline at end of file From 7bc7792449bf25ef2fbe20834ba1ab2113060457 Mon Sep 17 00:00:00 2001 From: TristanWolfram Date: Sun, 28 Jan 2024 13:51:04 +0100 Subject: [PATCH 07/31] Add pragma once directive and update plot objects in pdaf_test.cpp --- .../include/vortex_filtering/filters/pdaf.hpp | 2 + vortex-filtering/test/pdaf_test.cpp | 70 +++++++++++-------- 2 files changed, 41 insertions(+), 31 deletions(-) diff --git a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp index a17ffd79..63587005 100644 --- a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp @@ -1,3 +1,5 @@ +# pragma once + #include #include #include diff --git a/vortex-filtering/test/pdaf_test.cpp b/vortex-filtering/test/pdaf_test.cpp index 2ef57e1d..18be9f6f 100644 --- a/vortex-filtering/test/pdaf_test.cpp +++ b/vortex-filtering/test/pdaf_test.cpp @@ -298,38 +298,43 @@ TEST(PDAF, predict_next_state_is_calculating) gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; gp << "set size ratio -1\n"; gp << "set style circle radius 0.05\n"; + gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; gp.send1d(meas); int object_counter = 0; - gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 << "fs empty border lc rgb 'black'\n"; - gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 << "fs empty border lc rgb 'pink'\n"; + // Create the ellipses first + // for (const auto& state: x_updated) + // { + // vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2,2)); + // vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); + + // gp << "set object " << ++object_counter + // << " ellipse center " << ellipse.x << "," << ellipse.y + // << " size " << ellipse.a << "," << ellipse.b + // << " angle " << ellipse.angle + // << " fs solid 0.2 noborder front lc rgb 'blue'\n"; + // } + + // Create other objects + gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'black'\n"; + gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'pink'\n"; gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; - gp << "replot\n"; + // Add arrows gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " nohead lc rgb 'pink'\n"; gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n"; - gp << "replot\n"; + // Set gate ellipse vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); - gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle - << "fs empty border lc rgb 'cyan'\n"; - gp << "replot\n"; + << " fs empty border lc rgb 'cyan'\n"; - for (const auto& state: x_updated) - { - vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2,2)); - vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); + + // Execute the plot with all objects + gp << "replot\n"; - gp << "set object " << ++object_counter - << " ellipse center " << ellipse.x << "," << ellipse.y - << " size " << ellipse.a << "," << ellipse.b - << " angle " << ellipse.angle - << "fs solid 0.2 noborder front lc rgb 'blue'\n"; - }; - gp << "replot\n"; } TEST(PDAF, predict_next_state_2) @@ -358,13 +363,28 @@ TEST(PDAF, predict_next_state_2) std::cout << "x_final: " << x_final.mean() << std::endl; Gnuplot gp; + int object_counter = 0; + gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; gp << "set size ratio -1\n"; + + for (const auto& state: x_updated) + { + vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2,2)); + vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); + + gp << "set object " << ++object_counter + << " ellipse center " << ellipse.x << "," << ellipse.y + << " size " << ellipse.a << "," << ellipse.b + << " angle " << ellipse.angle + << "fs solid 0.2 noborder front lc rgb 'blue'\n"; + }; + gp << "set style circle radius 0.05\n"; gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; gp.send1d(meas); - int object_counter = 0; + gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 << "fs empty border lc rgb 'black'\n"; gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 << "fs empty border lc rgb 'pink'\n"; @@ -380,18 +400,6 @@ TEST(PDAF, predict_next_state_2) gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle << "fs empty border lc rgb 'cyan'\n"; - for (const auto& state: x_updated) - { - vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2,2)); - vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); - - gp << "set object " << ++object_counter - << " ellipse center " << ellipse.x << "," << ellipse.y - << " size " << ellipse.a << "," << ellipse.b - << " angle " << ellipse.angle - << "fs solid 0.2 noborder lc rgb 'blue'\n"; - }; - gp << "replot\n"; } \ No newline at end of file From d51fad9f6debe3190d812b8600f3434509503537 Mon Sep 17 00:00:00 2001 From: Sondre Selberg Date: Sun, 28 Jan 2024 17:17:47 +0100 Subject: [PATCH 08/31] ipda impl --- .../include/vortex_filtering/filters/ipda.hpp | 43 +++++++++++++++++++ 1 file changed, 43 insertions(+) create mode 100644 vortex-filtering/include/vortex_filtering/filters/ipda.hpp diff --git a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp new file mode 100644 index 00000000..31f96c9f --- /dev/null +++ b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp @@ -0,0 +1,43 @@ +#ifndef IPDA_HPP +#define IPDA_HPP + +#include +#include + +class IPDAFilter { +public: + IPDAFilter(); + double generate_new_probability(std::vector &measurements, double &probability_of_survival, float &probability_of_detection, double &standard_deviation, float &lambda); +private: + double last_detection_probability_; + double get_last_detection_probability(); + double sum_of_gaussian_probabilities(std::vector &measurements, double &standard_deviation); +}; + +IPDAFilter::IPDAFilter() { + last_detection_probability_ = 0.0; +} + +/// @brief +/// @param measurements Measurements to iterate over +/// @param probability_of_survival How likely the object is to survive (Ps) +/// @param probability_of_detection How likely the object is to be detected (Pd) +/// @param standard_deviation Standard deviation of the measurements (Sk) +/// @param lambda Lambda value for the Poisson distribution (Lambda) +double IPDAFilter::generate_new_probability(std::vector &measurements, double &probability_of_survival, float &probability_of_detection, double &standard_deviation, float &lambda) { + double predicted_existence_probability = probability_of_survival * last_detection_probability_; + double l_k = 1 - probability_of_detection + probability_of_detection / lambda * sum_of_gaussian_probabilities(measurements, standard_deviation); + + return (l_k * predicted_existence_probability) / (1 - (1 - l_k) * predicted_existence_probability); +} +double IPDAFilter::sum_of_gaussian_probabilities(std::vector &measurements, double &standard_deviation) { + double sum = 0.0; + + return sum; +} + +double IPDAFilter::get_last_detection_probability() { + return last_detection_probability_; +} + +#endif // IPDA_HPP From ba2d8445c7a49542036c29a431298764a729112e Mon Sep 17 00:00:00 2001 From: TristanWolfram Date: Sun, 28 Jan 2024 17:52:29 +0100 Subject: [PATCH 09/31] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Refactor=20plotting?= =?UTF-8?q?=20code=20in=20pdaf=5Ftest.cpp?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- vortex-filtering/test/pdaf_test.cpp | 67 +++++++++++++---------------- 1 file changed, 29 insertions(+), 38 deletions(-) diff --git a/vortex-filtering/test/pdaf_test.cpp b/vortex-filtering/test/pdaf_test.cpp index 18be9f6f..2dfca9af 100644 --- a/vortex-filtering/test/pdaf_test.cpp +++ b/vortex-filtering/test/pdaf_test.cpp @@ -297,44 +297,39 @@ TEST(PDAF, predict_next_state_is_calculating) Gnuplot gp; gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; gp << "set size ratio -1\n"; - gp << "set style circle radius 0.05\n"; + gp << "set style circle radius 0.05\n"; gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; gp.send1d(meas); int object_counter = 0; - // Create the ellipses first - // for (const auto& state: x_updated) - // { - // vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2,2)); - // vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); + for (const auto& state: x_updated) + { + vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2,2)); + vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); + + gp << "set object " << ++object_counter + << " ellipse center " << ellipse.x << "," << ellipse.y + << " size " << ellipse.a << "," << ellipse.b + << " angle " << ellipse.angle + << " front fc rgb 'skyblue' fs transparent solid 0.2 noborder\n"; - // gp << "set object " << ++object_counter - // << " ellipse center " << ellipse.x << "," << ellipse.y - // << " size " << ellipse.a << "," << ellipse.b - // << " angle " << ellipse.angle - // << " fs solid 0.2 noborder front lc rgb 'blue'\n"; - // } + gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; + } - // Create other objects gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'black'\n"; gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'pink'\n"; gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; - // Add arrows gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " nohead lc rgb 'pink'\n"; gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n"; - // Set gate ellipse vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle << " fs empty border lc rgb 'cyan'\n"; - - // Execute the plot with all objects gp << "replot\n"; - } TEST(PDAF, predict_next_state_2) @@ -363,43 +358,39 @@ TEST(PDAF, predict_next_state_2) std::cout << "x_final: " << x_final.mean() << std::endl; Gnuplot gp; - int object_counter = 0; - gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; gp << "set size ratio -1\n"; + gp << "set style circle radius 0.05\n"; + gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; + gp.send1d(meas); + + int object_counter = 0; + for (const auto& state: x_updated) { vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2,2)); vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); gp << "set object " << ++object_counter - << " ellipse center " << ellipse.x << "," << ellipse.y - << " size " << ellipse.a << "," << ellipse.b - << " angle " << ellipse.angle - << "fs solid 0.2 noborder front lc rgb 'blue'\n"; - }; - - gp << "set style circle radius 0.05\n"; - gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; - gp.send1d(meas); + << " ellipse center " << ellipse.x << "," << ellipse.y + << " size " << ellipse.a << "," << ellipse.b + << " angle " << ellipse.angle + << " front fc rgb 'skyblue' fs transparent solid 0.2 noborder\n"; - + gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; + } - gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 << "fs empty border lc rgb 'black'\n"; - gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 << "fs empty border lc rgb 'pink'\n"; + gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'black'\n"; + gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'pink'\n"; gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; - gp << "replot\n"; gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " nohead lc rgb 'pink'\n"; gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n"; - gp << "replot\n"; vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); - gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle - << "fs empty border lc rgb 'cyan'\n"; + << " fs empty border lc rgb 'cyan'\n"; - gp << "replot\n"; - + gp << "replot\n"; } \ No newline at end of file From 0e4d172a2390194261bc73fdf138246c71246642 Mon Sep 17 00:00:00 2001 From: TristanWolfram Date: Tue, 30 Jan 2024 19:17:16 +0100 Subject: [PATCH 10/31] =?UTF-8?q?=E2=9C=85=20added=20more=20tests?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/vortex_filtering/filters/pdaf.hpp | 1 + vortex-filtering/test/pdaf_test.cpp | 295 +++++++++++++++++- 2 files changed, 293 insertions(+), 3 deletions(-) diff --git a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp index 63587005..2eb0a501 100644 --- a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp @@ -52,6 +52,7 @@ class PDAF { x_updated.push_back(EKF::update(sen_model, x_pred, z_pred, measurement)); } + // TODO: return gaussian mixture Gauss_x x_final = get_weighted_average( z_meas, x_updated, diff --git a/vortex-filtering/test/pdaf_test.cpp b/vortex-filtering/test/pdaf_test.cpp index 2dfca9af..22d19ebb 100644 --- a/vortex-filtering/test/pdaf_test.cpp +++ b/vortex-filtering/test/pdaf_test.cpp @@ -211,7 +211,7 @@ TEST(PDAF, apply_gate_is_calculating) TEST(PDAF, apply_gate_is_separating_correctly) { - double gate_threshold = 2; + double gate_threshold = 3; SimplePDAF pdaf; Eigen::Matrix2d cov; @@ -266,6 +266,24 @@ TEST(PDAF, apply_gate_is_separating_correctly_2) EXPECT_EQ(inside.size(), 5u); EXPECT_EQ(outside.size(), 1u); + + Gnuplot gp; + gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; + gp << "set size ratio -1\n"; + gp << "set style circle radius 0.05\n"; + gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; + gp.send1d(meas); + + int object_counter = 0; + + gp << "set object " << ++object_counter << " circle center " << z_pred.mean()(0) << "," << z_pred.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; + gp << "replot\n"; + + vortex::plotting::Ellipse prediction = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); + + gp << "set object " << ++object_counter << " ellipse center " << prediction.x << "," << prediction.y << " size " << prediction.a << "," << prediction.b << " angle " << prediction.angle + << "fs empty border lc rgb 'cyan'\n"; + gp << "replot\n"; } // testing the predict_next_state function @@ -313,7 +331,7 @@ TEST(PDAF, predict_next_state_is_calculating) << " ellipse center " << ellipse.x << "," << ellipse.y << " size " << ellipse.a << "," << ellipse.b << " angle " << ellipse.angle - << " front fc rgb 'skyblue' fs transparent solid 0.2 noborder\n"; + << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; } @@ -376,7 +394,7 @@ TEST(PDAF, predict_next_state_2) << " ellipse center " << ellipse.x << "," << ellipse.y << " size " << ellipse.a << "," << ellipse.b << " angle " << ellipse.angle - << " front fc rgb 'skyblue' fs transparent solid 0.2 noborder\n"; + << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; } @@ -393,4 +411,275 @@ TEST(PDAF, predict_next_state_2) << " fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; +} + +TEST(PDAF, predict_next_state_3_1) +{ + + double gate_threshold = 4; + double prob_of_detection = 0.9; + double clutter_intensity = 1.0; + SimplePDAF pdaf; + + vortex::prob::Gauss4d x_est(Eigen::Vector4d(0.5, 0.5, -0.75, -0.75), Eigen::Matrix4d::Identity()); + std::vector meas = {{0.0, 0.5}, {0.2, 0.2}, {0.8, 2.3}, {0.5, 0.0}, {4.2, 2.7}, {1.4, 2.5}}; + + auto dyn_model = std::make_shared>(0.5); + auto sen_model = std::make_shared>(1.0); + + auto [x_final, inside, outside, x_pred, z_pred, x_updated] = pdaf.predict_next_state( + x_est, + meas, + 1.0, + dyn_model, + sen_model, + gate_threshold, + prob_of_detection, + clutter_intensity); + std::cout << "x_final: " << x_final.mean() << std::endl; + + Gnuplot gp; + gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; + gp << "set size ratio -1\n"; + + gp << "set style circle radius 0.05\n"; + gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; + gp.send1d(meas); + + int object_counter = 0; + + for (const auto& state: x_updated) + { + vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2,2)); + vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); + + gp << "set object " << ++object_counter + << " ellipse center " << ellipse.x << "," << ellipse.y + << " size " << ellipse.a << "," << ellipse.b + << " angle " << ellipse.angle + << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + + gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; + } + + gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'black'\n"; + gp << "set object " << ++object_counter << " circle center " << 0.5 << "," << 0.5 << " size " << 0.05 << " fs empty border lc rgb 'black'\n"; + gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'pink'\n"; + gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; + + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " nohead lc rgb 'pink'\n"; + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n"; + + vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); + gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle + << " fs empty border lc rgb 'cyan'\n"; + + gp << "replot\n"; +} + +TEST(PDAF, predict_next_state_3_2) +{ + + double gate_threshold = 4; + double prob_of_detection = 0.9; + double clutter_intensity = 1.0; + SimplePDAF pdaf; + + vortex::prob::Gauss4d x_est(Eigen::Vector4d(-0.00173734, 0.0766262, -0.614584, -0.57184), Eigen::Matrix4d::Identity()); + std::vector meas = {{-0.5, 2.0}, {-0.23, 0.5}, {-2.0, 3.4}, {0.0, 1.3}, {0.14, 0.5}, {-2.5, 0.89}}; + + auto dyn_model = std::make_shared>(0.5); + auto sen_model = std::make_shared>(1.0); + + auto [x_final, inside, outside, x_pred, z_pred, x_updated] = pdaf.predict_next_state( + x_est, + meas, + 1.0, + dyn_model, + sen_model, + gate_threshold, + prob_of_detection, + clutter_intensity); + std::cout << "x_final: " << x_final.mean() << std::endl; + + Gnuplot gp; + gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; + gp << "set size ratio -1\n"; + + gp << "set style circle radius 0.05\n"; + gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; + gp.send1d(meas); + + int object_counter = 0; + + for (const auto& state: x_updated) + { + vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2,2)); + vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); + + gp << "set object " << ++object_counter + << " ellipse center " << ellipse.x << "," << ellipse.y + << " size " << ellipse.a << "," << ellipse.b + << " angle " << ellipse.angle + << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + + gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; + } + + gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'black'\n"; + gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'pink'\n"; + gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; + + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " nohead lc rgb 'pink'\n"; + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n"; + + // old state from 3_1 + gp << "set object " << ++object_counter << " circle center " << 0.5 << "," << 0.5 << " size " << 0.05 << " fs empty border lc rgb 'orange-red'\n"; + gp << "set arrow from " << 0.5 << "," << 0.5 << " to " << -0.00173734 << "," << 0.0766262 << " nohead lc rgb 'orange-red'\n"; + + vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); + gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle + << " fs empty border lc rgb 'cyan'\n"; + + gp << "replot\n"; +} + +TEST(PDAF, predict_next_state_3_3) +{ + + double gate_threshold = 4; + double prob_of_detection = 0.9; + double clutter_intensity = 1.0; + SimplePDAF pdaf; + + vortex::prob::Gauss4d x_est(Eigen::Vector4d(-0.55929, 0.0694888, -0.583476, -0.26382), Eigen::Matrix4d::Identity()); + std::vector meas = {{-1.5, 2.5}, {-1.2, 2.7}, {-0.8, 2.3},{-1.7, 1.9}, {-2.0, 3.0}, {-1.11, 2.04}}; + + auto dyn_model = std::make_shared>(0.5); + auto sen_model = std::make_shared>(1.0); + + auto [x_final, inside, outside, x_pred, z_pred, x_updated] = pdaf.predict_next_state( + x_est, + meas, + 1.0, + dyn_model, + sen_model, + gate_threshold, + prob_of_detection, + clutter_intensity); + std::cout << "x_final: " << x_final.mean() << std::endl; + + Gnuplot gp; + gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; + gp << "set size ratio -1\n"; + + gp << "set style circle radius 0.05\n"; + gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; + gp.send1d(meas); + + int object_counter = 0; + + for (const auto& state: x_updated) + { + vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2,2)); + vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); + + gp << "set object " << ++object_counter + << " ellipse center " << ellipse.x << "," << ellipse.y + << " size " << ellipse.a << "," << ellipse.b + << " angle " << ellipse.angle + << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + + gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; + } + + gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'black'\n"; + gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'pink'\n"; + gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; + + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " nohead lc rgb 'pink'\n"; + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n"; + + // old state from 3_1, 3_2 + gp << "set object " << ++object_counter << " circle center " << 0.5 << "," << 0.5 << " size " << 0.05 << " fs empty border lc rgb 'orange-red'\n"; + gp << "set arrow from " << 0.5 << "," << 0.5 << " to " << -0.00173734 << "," << 0.0766262 << " nohead lc rgb 'orange-red'\n"; + gp << "set object " << ++object_counter << " circle center " << -0.00173734 << "," << 0.0766262 << " size " << 0.05 << " fs empty border lc rgb 'orange-red'\n"; + gp << "set arrow from " << -0.00173734 << "," << 0.0766262 << " to " << -0.55929 << "," << 0.0694888 << " nohead lc rgb 'orange-red'\n"; + + vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); + gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle + << " fs empty border lc rgb 'cyan'\n"; + + gp << "replot\n"; +} + +TEST(PDAF, predict_next_state_3_4) +{ + + double gate_threshold = 4; + double prob_of_detection = 0.9; + double clutter_intensity = 1.0; + SimplePDAF pdaf; + + vortex::prob::Gauss4d x_est(Eigen::Vector4d(-1.20613, 0.610616, -0.618037, 0.175242), Eigen::Matrix4d::Identity()); + std::vector meas = {{-2.0, 2.2}, {-1.8, 2.3}, {-2.3, 2.0}, {0.6, 1.5}, {-2.0, 2.0}, {-1.4, 2.5}}; + + auto dyn_model = std::make_shared>(0.5); + auto sen_model = std::make_shared>(1.0); + + auto [x_final, inside, outside, x_pred, z_pred, x_updated] = pdaf.predict_next_state( + x_est, + meas, + 1.0, + dyn_model, + sen_model, + gate_threshold, + prob_of_detection, + clutter_intensity); + std::cout << "x_final: " << x_final.mean() << std::endl; + + Gnuplot gp; + gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; + gp << "set size ratio -1\n"; + + gp << "set style circle radius 0.05\n"; + gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; + gp.send1d(meas); + + int object_counter = 0; + + for (const auto& state: x_updated) + { + vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2,2)); + vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); + + gp << "set object " << ++object_counter + << " ellipse center " << ellipse.x << "," << ellipse.y + << " size " << ellipse.a << "," << ellipse.b + << " angle " << ellipse.angle + << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + + gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; + } + + gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'black'\n"; + gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'pink'\n"; + gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; + + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " nohead lc rgb 'pink'\n"; + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n"; + + // old state from 3_1, 3_2, 3_3 + gp << "set object " << ++object_counter << " circle center " << 0.5 << "," << 0.5 << " size " << 0.05 << " fs empty border lc rgb 'orange-red'\n"; + gp << "set arrow from " << 0.5 << "," << 0.5 << " to " << -0.00173734 << "," << 0.0766262 << " nohead lc rgb 'orange-red'\n"; + gp << "set object " << ++object_counter << " circle center " << -0.00173734 << "," << 0.0766262 << " size " << 0.05 << " fs empty border lc rgb 'orange-red'\n"; + gp << "set arrow from " << -0.00173734 << "," << 0.0766262 << " to " << -0.55929 << "," << 0.0694888 << " nohead lc rgb 'orange-red'\n"; + gp << "set object " << ++object_counter << " circle center " << -0.55929 << "," << 0.0694888 << " size " << 0.05 << " fs empty border lc rgb 'orange-red'\n"; + gp << "set arrow from " << -0.55929 << "," << 0.0694888 << " to " << -1.20613 << "," << 0.610616 << " nohead lc rgb 'orange-red'\n"; + + vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); + gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle + << " fs empty border lc rgb 'cyan'\n"; + + gp << "replot\n"; } \ No newline at end of file From 87ffbf7419c44f6269d4d7b00f78d3ebfd2f8a78 Mon Sep 17 00:00:00 2001 From: Clang Robot Date: Tue, 30 Jan 2024 18:19:18 +0000 Subject: [PATCH 11/31] Committing clang-format changes --- .../include/vortex_filtering/filters/ipda.hpp | 43 +- .../include/vortex_filtering/filters/pdaf.hpp | 200 ++-- vortex-filtering/test/pdaf_test.cpp | 1000 ++++++++--------- 3 files changed, 559 insertions(+), 684 deletions(-) diff --git a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp index 31f96c9f..1ac5978e 100644 --- a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp @@ -1,43 +1,44 @@ #ifndef IPDA_HPP #define IPDA_HPP -#include #include +#include class IPDAFilter { public: - IPDAFilter(); - double generate_new_probability(std::vector &measurements, double &probability_of_survival, float &probability_of_detection, double &standard_deviation, float &lambda); + IPDAFilter(); + double generate_new_probability(std::vector &measurements, double &probability_of_survival, float &probability_of_detection, + double &standard_deviation, float &lambda); + private: - double last_detection_probability_; - double get_last_detection_probability(); - double sum_of_gaussian_probabilities(std::vector &measurements, double &standard_deviation); + double last_detection_probability_; + double get_last_detection_probability(); + double sum_of_gaussian_probabilities(std::vector &measurements, double &standard_deviation); }; -IPDAFilter::IPDAFilter() { - last_detection_probability_ = 0.0; -} +IPDAFilter::IPDAFilter() { last_detection_probability_ = 0.0; } -/// @brief +/// @brief /// @param measurements Measurements to iterate over /// @param probability_of_survival How likely the object is to survive (Ps) /// @param probability_of_detection How likely the object is to be detected (Pd) /// @param standard_deviation Standard deviation of the measurements (Sk) /// @param lambda Lambda value for the Poisson distribution (Lambda) -double IPDAFilter::generate_new_probability(std::vector &measurements, double &probability_of_survival, float &probability_of_detection, double &standard_deviation, float &lambda) { - double predicted_existence_probability = probability_of_survival * last_detection_probability_; - double l_k = 1 - probability_of_detection + probability_of_detection / lambda * sum_of_gaussian_probabilities(measurements, standard_deviation); +double IPDAFilter::generate_new_probability(std::vector &measurements, double &probability_of_survival, float &probability_of_detection, + double &standard_deviation, float &lambda) +{ + double predicted_existence_probability = probability_of_survival * last_detection_probability_; + double l_k = 1 - probability_of_detection + probability_of_detection / lambda * sum_of_gaussian_probabilities(measurements, standard_deviation); - return (l_k * predicted_existence_probability) / (1 - (1 - l_k) * predicted_existence_probability); -} -double IPDAFilter::sum_of_gaussian_probabilities(std::vector &measurements, double &standard_deviation) { - double sum = 0.0; - - return sum; + return (l_k * predicted_existence_probability) / (1 - (1 - l_k) * predicted_existence_probability); } +double IPDAFilter::sum_of_gaussian_probabilities(std::vector &measurements, double &standard_deviation) +{ + double sum = 0.0; -double IPDAFilter::get_last_detection_probability() { - return last_detection_probability_; + return sum; } +double IPDAFilter::get_last_detection_probability() { return last_detection_probability_; } + #endif // IPDA_HPP diff --git a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp index 2eb0a501..dc80a43e 100644 --- a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp @@ -1,4 +1,4 @@ -# pragma once +#pragma once #include #include @@ -13,127 +13,97 @@ namespace filter { using std::string; // using std::endl; -template -class PDAF { +template class PDAF { public: - using SensModI = typename SensModT::SensModI; - using DynModI = typename DynModT::DynModI; - using DynModPtr = std::shared_ptr; - using SensModPtr = std::shared_ptr; - using EKF = vortex::filter::EKF; - using Gauss_z = typename SensModI::Gauss_z; - using Gauss_x = typename DynModI::Gauss_x; - using Vec_z = typename SensModI::Vec_z; - using MeasurementsZd = std::vector; - using StatesXd = std::vector; - using GaussMixZd = vortex::prob::GaussianMixture; - - PDAF() - { - std::cout << "Created PDAF class!" << std::endl; + using SensModI = typename SensModT::SensModI; + using DynModI = typename DynModT::DynModI; + using DynModPtr = std::shared_ptr; + using SensModPtr = std::shared_ptr; + using EKF = vortex::filter::EKF; + using Gauss_z = typename SensModI::Gauss_z; + using Gauss_x = typename DynModI::Gauss_x; + using Vec_z = typename SensModI::Vec_z; + using MeasurementsZd = std::vector; + using StatesXd = std::vector; + using GaussMixZd = vortex::prob::GaussianMixture; + + PDAF() { std::cout << "Created PDAF class!" << std::endl; } + + std::tuple predict_next_state(const Gauss_x &x_est, const MeasurementsZd &z_meas, + double timestep, const DynModPtr &dyn_model, + const SensModPtr &sen_model, double gate_threshold, + double prob_of_detection, double clutter_intensity) const + { + auto [x_pred, z_pred] = EKF::predict(dyn_model, sen_model, timestep, x_est); + auto [inside, outside] = apply_gate(z_meas, z_pred, gate_threshold); + + StatesXd x_updated; + for (const auto &measurement : inside) { + x_updated.push_back(EKF::update(sen_model, x_pred, z_pred, measurement)); } - std::tuple predict_next_state( - const Gauss_x& x_est, - const MeasurementsZd& z_meas, - double timestep, - const DynModPtr& dyn_model, - const SensModPtr& sen_model, - double gate_threshold, - double prob_of_detection, - double clutter_intensity - ) const - { - auto [x_pred, z_pred] = EKF::predict(dyn_model, sen_model, timestep, x_est); - auto [inside, outside] = apply_gate(z_meas, z_pred, gate_threshold); - - StatesXd x_updated; - for (const auto& measurement : inside) { - x_updated.push_back(EKF::update(sen_model, x_pred, z_pred, measurement)); - } - - // TODO: return gaussian mixture - Gauss_x x_final = get_weighted_average( - z_meas, - x_updated, - z_pred, - x_pred, - prob_of_detection, - clutter_intensity); - return {x_final, inside, outside, x_pred, z_pred, x_updated}; + // TODO: return gaussian mixture + Gauss_x x_final = get_weighted_average(z_meas, x_updated, z_pred, x_pred, prob_of_detection, clutter_intensity); + return {x_final, inside, outside, x_pred, z_pred, x_updated}; + } + + std::tuple apply_gate(const MeasurementsZd &z_meas, const Gauss_z &z_pred, double gate_threshold) const + { + MeasurementsZd inside_meas; + MeasurementsZd outside_meas; + + for (const auto &measurement : z_meas) { + double distance = z_pred.mahalanobis_distance(measurement); + std::cout << "measurement: " << measurement << std::endl; + std::cout << "z_pred: " << z_pred.mean() << std::endl; + std::cout << "distance: " << distance << std::endl; + + if (distance <= gate_threshold) { + inside_meas.push_back(measurement); + } + else { + outside_meas.push_back(measurement); + } } - std::tuple apply_gate( - const MeasurementsZd& z_meas, - const Gauss_z& z_pred, - double gate_threshold - ) const - { - MeasurementsZd inside_meas; - MeasurementsZd outside_meas; - - for (const auto& measurement : z_meas) { - double distance = z_pred.mahalanobis_distance(measurement); - std::cout << "measurement: " << measurement << std::endl; - std::cout << "z_pred: " << z_pred.mean() << std::endl; - std::cout << "distance: " << distance << std::endl; - - if (distance <= gate_threshold) { - inside_meas.push_back(measurement); - } else { - outside_meas.push_back(measurement); - } - } - - return {inside_meas, outside_meas}; - } + return {inside_meas, outside_meas}; + } - // Getting weighted average of the predicted states - Gauss_x get_weighted_average( - const MeasurementsZd& z_meas, - const StatesXd& updated_states, - const Gauss_z& z_pred, - const Gauss_x& x_pred, - double prob_of_detection, - double clutter_intensity - ) const - { - StatesXd states; - states.push_back(x_pred); - states.insert(states.end(), updated_states.begin(), updated_states.end()); - - Eigen::VectorXd weights = get_weights(z_meas, z_pred, prob_of_detection, clutter_intensity); - - GaussMixZd gaussian_mixture(weights, states); - - return gaussian_mixture.reduce(); - } + // Getting weighted average of the predicted states + Gauss_x get_weighted_average(const MeasurementsZd &z_meas, const StatesXd &updated_states, const Gauss_z &z_pred, const Gauss_x &x_pred, + double prob_of_detection, double clutter_intensity) const + { + StatesXd states; + states.push_back(x_pred); + states.insert(states.end(), updated_states.begin(), updated_states.end()); + + Eigen::VectorXd weights = get_weights(z_meas, z_pred, prob_of_detection, clutter_intensity); + + GaussMixZd gaussian_mixture(weights, states); + + return gaussian_mixture.reduce(); + } - // Getting association probabilities according to textbook p. 123 "Corollary 7.3.3" - Eigen::VectorXd get_weights( - const MeasurementsZd& z_meas, - const Gauss_z& z_pred, - double prob_of_detection, - double clutter_intensity - ) const - { - Eigen::VectorXd weights(z_meas.size() + 1); - - // in case no measurement assosiates with the target - double no_association = clutter_intensity * (1 - prob_of_detection); - weights(0) = no_association; - - // measurements associating with the target - for (size_t k = 1; k < z_meas.size() + 1; k++) { - weights(k) = (prob_of_detection * z_pred.pdf(z_meas.at(k - 1))); - } - - // normalize weights - weights /= weights.sum(); - - return weights; + // Getting association probabilities according to textbook p. 123 "Corollary 7.3.3" + Eigen::VectorXd get_weights(const MeasurementsZd &z_meas, const Gauss_z &z_pred, double prob_of_detection, double clutter_intensity) const + { + Eigen::VectorXd weights(z_meas.size() + 1); + + // in case no measurement assosiates with the target + double no_association = clutter_intensity * (1 - prob_of_detection); + weights(0) = no_association; + + // measurements associating with the target + for (size_t k = 1; k < z_meas.size() + 1; k++) { + weights(k) = (prob_of_detection * z_pred.pdf(z_meas.at(k - 1))); } + + // normalize weights + weights /= weights.sum(); + + return weights; + } }; -} // namespace filter -} // namespace vortex \ No newline at end of file +} // namespace filter +} // namespace vortex \ No newline at end of file diff --git a/vortex-filtering/test/pdaf_test.cpp b/vortex-filtering/test/pdaf_test.cpp index 22d19ebb..243ad81f 100644 --- a/vortex-filtering/test/pdaf_test.cpp +++ b/vortex-filtering/test/pdaf_test.cpp @@ -1,685 +1,589 @@ +#include #include +#include #include #include -#include -#include using SimplePDAF = vortex::filter::PDAF, vortex::models::IdentitySensorModel<4, 2>>; -TEST(PDAF, init) -{ - SimplePDAF pdaf; -} - +TEST(PDAF, init) { SimplePDAF pdaf; } // testing the get_weights function TEST(PDAF, get_weights_is_calculating) { - SimplePDAF pdaf; + SimplePDAF pdaf; - double prob_of_detection = 0.8; - double clutter_intensity = 1.0; + double prob_of_detection = 0.8; + double clutter_intensity = 1.0; - vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); - std::vector meas = {{0.0, 0.0}, {2.0, 1.0}}; + vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); + std::vector meas = {{0.0, 0.0}, {2.0, 1.0}}; - Eigen::VectorXd weights = pdaf.get_weights(meas, z_pred, prob_of_detection, clutter_intensity); + Eigen::VectorXd weights = pdaf.get_weights(meas, z_pred, prob_of_detection, clutter_intensity); - std::cout << "weights: " << weights << std::endl; + std::cout << "weights: " << weights << std::endl; - EXPECT_EQ(weights.size(), 3); + EXPECT_EQ(weights.size(), 3); } TEST(PDAF, if_no_clutter_first_weight_is_zero) { - SimplePDAF pdaf; + SimplePDAF pdaf; - double prob_of_detection = 0.8; - double clutter_intensity = 0.0; + double prob_of_detection = 0.8; + double clutter_intensity = 0.0; - vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); - std::vector meas = {{0.0, 0.0}, {2.0, 1.0}}; + vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); + std::vector meas = {{0.0, 0.0}, {2.0, 1.0}}; - Eigen::VectorXd weights = pdaf.get_weights(meas, z_pred, prob_of_detection, clutter_intensity); + Eigen::VectorXd weights = pdaf.get_weights(meas, z_pred, prob_of_detection, clutter_intensity); - std::cout << "weights: " << weights << std::endl; + std::cout << "weights: " << weights << std::endl; - EXPECT_EQ(weights(0), 0.0); -} + EXPECT_EQ(weights(0), 0.0); +} TEST(PDAF, weights_are_decreasing_with_distance) { - SimplePDAF pdaf; + SimplePDAF pdaf; - double prob_of_detection = 0.8; - double clutter_intensity = 1.0; + double prob_of_detection = 0.8; + double clutter_intensity = 1.0; - vortex::prob::Gauss2d z_pred(Eigen::Vector2d(1.0, 1.0), Eigen::Matrix2d::Identity()); - std::vector meas = {{2.0, 1.0}, {3.0, 1.0}, {4.0, 1.0}}; + vortex::prob::Gauss2d z_pred(Eigen::Vector2d(1.0, 1.0), Eigen::Matrix2d::Identity()); + std::vector meas = {{2.0, 1.0}, {3.0, 1.0}, {4.0, 1.0}}; - Eigen::VectorXd weights = pdaf.get_weights(meas, z_pred, prob_of_detection, clutter_intensity); + Eigen::VectorXd weights = pdaf.get_weights(meas, z_pred, prob_of_detection, clutter_intensity); - std::cout << "weights: " << weights << std::endl; + std::cout << "weights: " << weights << std::endl; - EXPECT_GT(weights(1), weights(2)); - EXPECT_GT(weights(2), weights(3)); + EXPECT_GT(weights(1), weights(2)); + EXPECT_GT(weights(2), weights(3)); } - // testing the get_weighted_average function TEST(PDAF, get_weighted_average_is_calculating) { - SimplePDAF pdaf; - - double prob_of_detection = 0.8; - double clutter_intensity = 1.0; - - vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); - vortex::prob::Gauss4d x_pred(Eigen::Vector4d(0.0, 0.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); - std::vector meas = {{0.0, 0.0}, {2.0, 1.0}}; - std::vector updated_states = { - vortex::prob::Gauss4d(Eigen::Vector4d(0.0, 0.0, 0.0, 0.0), Eigen::Matrix4d::Identity()), - vortex::prob::Gauss4d(Eigen::Vector4d(1.0, 1.0, 1.0, 1.0), Eigen::Matrix4d::Identity()) - }; - - vortex::prob::Gauss4d weighted_average = pdaf.get_weighted_average( - meas, - updated_states, - z_pred, x_pred, - prob_of_detection, - clutter_intensity - ); - - std::cout << "weighted average: " << weighted_average.mean() << std::endl; + SimplePDAF pdaf; + + double prob_of_detection = 0.8; + double clutter_intensity = 1.0; + + vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); + vortex::prob::Gauss4d x_pred(Eigen::Vector4d(0.0, 0.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); + std::vector meas = {{0.0, 0.0}, {2.0, 1.0}}; + std::vector updated_states = {vortex::prob::Gauss4d(Eigen::Vector4d(0.0, 0.0, 0.0, 0.0), Eigen::Matrix4d::Identity()), + vortex::prob::Gauss4d(Eigen::Vector4d(1.0, 1.0, 1.0, 1.0), Eigen::Matrix4d::Identity())}; + + vortex::prob::Gauss4d weighted_average = pdaf.get_weighted_average(meas, updated_states, z_pred, x_pred, prob_of_detection, clutter_intensity); + + std::cout << "weighted average: " << weighted_average.mean() << std::endl; } TEST(PDAF, average_state_is_in_between_prediction_and_measurement_y_axis) { - SimplePDAF pdaf; - - double prob_of_detection = 0.8; - double clutter_intensity = 1.0; - - vortex::prob::Gauss2d z_pred(Eigen::Vector2d(1.0, 1.0), Eigen::Matrix2d::Identity()); - vortex::prob::Gauss4d x_pred(Eigen::Vector4d(1.0, 1.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); - std::vector meas = {{1.0, 2.0}}; - std::vector updated_states = { - vortex::prob::Gauss4d(Eigen::Vector4d(1.0, 1.5, 0.0, 0.0), Eigen::Matrix4d::Identity()) - }; - - vortex::prob::Gauss4d weighted_average = pdaf.get_weighted_average( - meas, - updated_states, - z_pred, - x_pred, - prob_of_detection, - clutter_intensity - ); - - EXPECT_GT(weighted_average.mean()(1), x_pred.mean()(1)); - EXPECT_GT(weighted_average.mean()(1), z_pred.mean()(1)); - EXPECT_LT(weighted_average.mean()(1), meas[0](1)); - EXPECT_LT(weighted_average.mean()(1), updated_states[0].mean()(1)); - - std::cout << "weighted average: " << weighted_average.mean() << std::endl; + SimplePDAF pdaf; + + double prob_of_detection = 0.8; + double clutter_intensity = 1.0; + + vortex::prob::Gauss2d z_pred(Eigen::Vector2d(1.0, 1.0), Eigen::Matrix2d::Identity()); + vortex::prob::Gauss4d x_pred(Eigen::Vector4d(1.0, 1.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); + std::vector meas = {{1.0, 2.0}}; + std::vector updated_states = {vortex::prob::Gauss4d(Eigen::Vector4d(1.0, 1.5, 0.0, 0.0), Eigen::Matrix4d::Identity())}; + + vortex::prob::Gauss4d weighted_average = pdaf.get_weighted_average(meas, updated_states, z_pred, x_pred, prob_of_detection, clutter_intensity); + + EXPECT_GT(weighted_average.mean()(1), x_pred.mean()(1)); + EXPECT_GT(weighted_average.mean()(1), z_pred.mean()(1)); + EXPECT_LT(weighted_average.mean()(1), meas[0](1)); + EXPECT_LT(weighted_average.mean()(1), updated_states[0].mean()(1)); + + std::cout << "weighted average: " << weighted_average.mean() << std::endl; } TEST(PDAF, average_state_is_in_between_prediction_and_measurement_x_axis) { - SimplePDAF pdaf; - - double prob_of_detection = 0.8; - double clutter_intensity = 1.0; - - vortex::prob::Gauss2d z_pred(Eigen::Vector2d(1.0, 1.0), Eigen::Matrix2d::Identity()); - vortex::prob::Gauss4d x_pred(Eigen::Vector4d(1.0, 1.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); - std::vector meas = {{2.0, 1.0}}; - std::vector updated_states = { - vortex::prob::Gauss4d(Eigen::Vector4d(1.5, 1.0, 0.0, 0.0), Eigen::Matrix4d::Identity()) - }; - - vortex::prob::Gauss4d weighted_average = pdaf.get_weighted_average( - meas, - updated_states, - z_pred, - x_pred, - prob_of_detection, - clutter_intensity - ); - - EXPECT_GT(weighted_average.mean()(0), x_pred.mean()(0)); - EXPECT_GT(weighted_average.mean()(0), z_pred.mean()(0)); - EXPECT_LT(weighted_average.mean()(0), meas[0](0)); - EXPECT_LT(weighted_average.mean()(0), updated_states[0].mean()(0)); - - std::cout << "weighted average: " << weighted_average.mean() << std::endl; + SimplePDAF pdaf; + + double prob_of_detection = 0.8; + double clutter_intensity = 1.0; + + vortex::prob::Gauss2d z_pred(Eigen::Vector2d(1.0, 1.0), Eigen::Matrix2d::Identity()); + vortex::prob::Gauss4d x_pred(Eigen::Vector4d(1.0, 1.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); + std::vector meas = {{2.0, 1.0}}; + std::vector updated_states = {vortex::prob::Gauss4d(Eigen::Vector4d(1.5, 1.0, 0.0, 0.0), Eigen::Matrix4d::Identity())}; + + vortex::prob::Gauss4d weighted_average = pdaf.get_weighted_average(meas, updated_states, z_pred, x_pred, prob_of_detection, clutter_intensity); + + EXPECT_GT(weighted_average.mean()(0), x_pred.mean()(0)); + EXPECT_GT(weighted_average.mean()(0), z_pred.mean()(0)); + EXPECT_LT(weighted_average.mean()(0), meas[0](0)); + EXPECT_LT(weighted_average.mean()(0), updated_states[0].mean()(0)); + + std::cout << "weighted average: " << weighted_average.mean() << std::endl; } TEST(PDAF, average_state_is_in_between_prediction_and_measurement_both_axes) { - SimplePDAF pdaf; - - double prob_of_detection = 0.8; - double clutter_intensity = 1.0; - - vortex::prob::Gauss2d z_pred(Eigen::Vector2d(1.0, 1.0), Eigen::Matrix2d::Identity()); - vortex::prob::Gauss4d x_pred(Eigen::Vector4d(1.0, 1.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); - std::vector meas = {{2.0, 2.0}}; - std::vector updated_states = { - vortex::prob::Gauss4d(Eigen::Vector4d(1.5, 1.5, 0.0, 0.0), Eigen::Matrix4d::Identity()) - }; - - vortex::prob::Gauss4d weighted_average = pdaf.get_weighted_average( - meas, - updated_states, - z_pred, - x_pred, - prob_of_detection, - clutter_intensity - ); - - EXPECT_GT(weighted_average.mean()(0), x_pred.mean()(0)); - EXPECT_GT(weighted_average.mean()(0), z_pred.mean()(0)); - EXPECT_LT(weighted_average.mean()(0), meas[0](0)); - EXPECT_LT(weighted_average.mean()(0), updated_states[0].mean()(0)); - - EXPECT_GT(weighted_average.mean()(1), x_pred.mean()(1)); - EXPECT_GT(weighted_average.mean()(1), z_pred.mean()(1)); - EXPECT_LT(weighted_average.mean()(1), meas[0](1)); - EXPECT_LT(weighted_average.mean()(1), updated_states[0].mean()(1)); - - std::cout << "weighted average: " << weighted_average.mean() << std::endl; + SimplePDAF pdaf; + + double prob_of_detection = 0.8; + double clutter_intensity = 1.0; + + vortex::prob::Gauss2d z_pred(Eigen::Vector2d(1.0, 1.0), Eigen::Matrix2d::Identity()); + vortex::prob::Gauss4d x_pred(Eigen::Vector4d(1.0, 1.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); + std::vector meas = {{2.0, 2.0}}; + std::vector updated_states = {vortex::prob::Gauss4d(Eigen::Vector4d(1.5, 1.5, 0.0, 0.0), Eigen::Matrix4d::Identity())}; + + vortex::prob::Gauss4d weighted_average = pdaf.get_weighted_average(meas, updated_states, z_pred, x_pred, prob_of_detection, clutter_intensity); + + EXPECT_GT(weighted_average.mean()(0), x_pred.mean()(0)); + EXPECT_GT(weighted_average.mean()(0), z_pred.mean()(0)); + EXPECT_LT(weighted_average.mean()(0), meas[0](0)); + EXPECT_LT(weighted_average.mean()(0), updated_states[0].mean()(0)); + + EXPECT_GT(weighted_average.mean()(1), x_pred.mean()(1)); + EXPECT_GT(weighted_average.mean()(1), z_pred.mean()(1)); + EXPECT_LT(weighted_average.mean()(1), meas[0](1)); + EXPECT_LT(weighted_average.mean()(1), updated_states[0].mean()(1)); + + std::cout << "weighted average: " << weighted_average.mean() << std::endl; } // testing the apply_gate function TEST(PDAF, apply_gate_is_calculating) { - - SimplePDAF pdaf; + SimplePDAF pdaf; - double gate_threshold = 1.8; + double gate_threshold = 1.8; - vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); - std::vector meas = {{0.0, 1.0}, {1.0, 0.0}, {1.0, 1.0},{0.0, 2.0}, {2.0, 0.0}, {2.0, 2.0}}; + vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); + std::vector meas = {{0.0, 1.0}, {1.0, 0.0}, {1.0, 1.0}, {0.0, 2.0}, {2.0, 0.0}, {2.0, 2.0}}; - auto [inside, outside] = pdaf.apply_gate( - meas, - z_pred, - gate_threshold); + auto [inside, outside] = pdaf.apply_gate(meas, z_pred, gate_threshold); } TEST(PDAF, apply_gate_is_separating_correctly) { - double gate_threshold = 3; - SimplePDAF pdaf; - - Eigen::Matrix2d cov; - cov << 1.0, 0.0, - 0.0, 4.0; + double gate_threshold = 3; + SimplePDAF pdaf; - vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), cov); - std::vector meas = {{0.0, 4.0}, {4.0, 0.0}}; + Eigen::Matrix2d cov; + cov << 1.0, 0.0, 0.0, 4.0; - auto [inside, outside] = pdaf.apply_gate( - meas, - z_pred, - gate_threshold); + vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), cov); + std::vector meas = {{0.0, 4.0}, {4.0, 0.0}}; - EXPECT_EQ(inside.size(), 1u); - EXPECT_EQ(outside.size(), 1u); - EXPECT_EQ(inside[0], meas[0]); - EXPECT_EQ(outside[0], meas[1]); + auto [inside, outside] = pdaf.apply_gate(meas, z_pred, gate_threshold); - Gnuplot gp; - gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; - gp << "set size ratio -1\n"; - gp << "set style circle radius 0.05\n"; - gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; - gp.send1d(meas); + EXPECT_EQ(inside.size(), 1u); + EXPECT_EQ(outside.size(), 1u); + EXPECT_EQ(inside[0], meas[0]); + EXPECT_EQ(outside[0], meas[1]); - int object_counter = 0; + Gnuplot gp; + gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; + gp << "set size ratio -1\n"; + gp << "set style circle radius 0.05\n"; + gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; + gp.send1d(meas); - gp << "set object " << ++object_counter << " circle center " << z_pred.mean()(0) << "," << z_pred.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; - gp << "replot\n"; + int object_counter = 0; - vortex::plotting::Ellipse prediction = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); + gp << "set object " << ++object_counter << " circle center " << z_pred.mean()(0) << "," << z_pred.mean()(1) << " size " << 0.05 + << " fs empty border lc rgb 'green'\n"; + gp << "replot\n"; - gp << "set object " << ++object_counter << " ellipse center " << prediction.x << "," << prediction.y << " size " << prediction.a << "," << prediction.b << " angle " << prediction.angle - << "fs empty border lc rgb 'cyan'\n"; - gp << "replot\n"; + vortex::plotting::Ellipse prediction = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); + gp << "set object " << ++object_counter << " ellipse center " << prediction.x << "," << prediction.y << " size " << prediction.a << "," << prediction.b + << " angle " << prediction.angle << "fs empty border lc rgb 'cyan'\n"; + gp << "replot\n"; } TEST(PDAF, apply_gate_is_separating_correctly_2) { - double gate_threshold = 2.1; - SimplePDAF pdaf; + double gate_threshold = 2.1; + SimplePDAF pdaf; - vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); - std::vector meas = {{0.0, 1.0}, {1.0, 0.0}, {1.0, 1.0},{0.0, 2.0}, {2.0, 0.0}, {2.0, 2.0}}; + vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); + std::vector meas = {{0.0, 1.0}, {1.0, 0.0}, {1.0, 1.0}, {0.0, 2.0}, {2.0, 0.0}, {2.0, 2.0}}; - auto [inside, outside] = pdaf.apply_gate( - meas, - z_pred, - gate_threshold); + auto [inside, outside] = pdaf.apply_gate(meas, z_pred, gate_threshold); - EXPECT_EQ(inside.size(), 5u); - EXPECT_EQ(outside.size(), 1u); + EXPECT_EQ(inside.size(), 5u); + EXPECT_EQ(outside.size(), 1u); - Gnuplot gp; - gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; - gp << "set size ratio -1\n"; - gp << "set style circle radius 0.05\n"; - gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; - gp.send1d(meas); + Gnuplot gp; + gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; + gp << "set size ratio -1\n"; + gp << "set style circle radius 0.05\n"; + gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; + gp.send1d(meas); - int object_counter = 0; + int object_counter = 0; - gp << "set object " << ++object_counter << " circle center " << z_pred.mean()(0) << "," << z_pred.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; - gp << "replot\n"; + gp << "set object " << ++object_counter << " circle center " << z_pred.mean()(0) << "," << z_pred.mean()(1) << " size " << 0.05 + << " fs empty border lc rgb 'green'\n"; + gp << "replot\n"; - vortex::plotting::Ellipse prediction = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); + vortex::plotting::Ellipse prediction = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); - gp << "set object " << ++object_counter << " ellipse center " << prediction.x << "," << prediction.y << " size " << prediction.a << "," << prediction.b << " angle " << prediction.angle - << "fs empty border lc rgb 'cyan'\n"; - gp << "replot\n"; + gp << "set object " << ++object_counter << " ellipse center " << prediction.x << "," << prediction.y << " size " << prediction.a << "," << prediction.b + << " angle " << prediction.angle << "fs empty border lc rgb 'cyan'\n"; + gp << "replot\n"; } // testing the predict_next_state function TEST(PDAF, predict_next_state_is_calculating) { - double gate_threshold = 1.12; - double prob_of_detection = 0.8; - double clutter_intensity = 1.0; - SimplePDAF pdaf; + double gate_threshold = 1.12; + double prob_of_detection = 0.8; + double clutter_intensity = 1.0; + SimplePDAF pdaf; - vortex::prob::Gauss4d x_est(Eigen::Vector4d(0.0, 0.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); - std::vector meas = {{0.0, 1.0}, {1.0, 0.0}, {1.0, 1.0},{0.0, 2.0}, {2.0, 0.0}, {2.0, 2.0}}; + vortex::prob::Gauss4d x_est(Eigen::Vector4d(0.0, 0.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); + std::vector meas = {{0.0, 1.0}, {1.0, 0.0}, {1.0, 1.0}, {0.0, 2.0}, {2.0, 0.0}, {2.0, 2.0}}; - auto dyn_model = std::make_shared>(1.0); - auto sen_model = std::make_shared>(1.0); + auto dyn_model = std::make_shared>(1.0); + auto sen_model = std::make_shared>(1.0); - auto [x_final, inside, outside, x_pred, z_pred, x_updated] = pdaf.predict_next_state( - x_est, - meas, - 1.0, - dyn_model, - sen_model, - gate_threshold, - prob_of_detection, - clutter_intensity); - std::cout << "x_final: " << x_final.mean() << std::endl; + auto [x_final, inside, outside, x_pred, z_pred, x_updated] = + pdaf.predict_next_state(x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); + std::cout << "x_final: " << x_final.mean() << std::endl; - Gnuplot gp; - gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; - gp << "set size ratio -1\n"; + Gnuplot gp; + gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; + gp << "set size ratio -1\n"; - gp << "set style circle radius 0.05\n"; - gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; - gp.send1d(meas); + gp << "set style circle radius 0.05\n"; + gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; + gp.send1d(meas); - int object_counter = 0; + int object_counter = 0; - for (const auto& state: x_updated) - { - vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2,2)); - vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); + for (const auto &state : x_updated) { + vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2, 2)); + vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); - gp << "set object " << ++object_counter - << " ellipse center " << ellipse.x << "," << ellipse.y - << " size " << ellipse.a << "," << ellipse.b - << " angle " << ellipse.angle - << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + gp << "set object " << ++object_counter << " ellipse center " << ellipse.x << "," << ellipse.y << " size " << ellipse.a << "," << ellipse.b << " angle " + << ellipse.angle << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; - gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; - } + gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; + } - gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'black'\n"; - gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'pink'\n"; - gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; + gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 + << " fs empty border lc rgb 'black'\n"; + gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 + << " fs empty border lc rgb 'pink'\n"; + gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) << " size " << 0.05 + << " fs empty border lc rgb 'green'\n"; - gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " nohead lc rgb 'pink'\n"; - gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n"; + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " nohead lc rgb 'pink'\n"; + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n"; - vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); - gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle - << " fs empty border lc rgb 'cyan'\n"; + vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); + gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle + << " fs empty border lc rgb 'cyan'\n"; - gp << "replot\n"; + gp << "replot\n"; } TEST(PDAF, predict_next_state_2) { - double gate_threshold = 2; - double prob_of_detection = 0.8; - double clutter_intensity = 1.0; - SimplePDAF pdaf; + double gate_threshold = 2; + double prob_of_detection = 0.8; + double clutter_intensity = 1.0; + SimplePDAF pdaf; - vortex::prob::Gauss4d x_est(Eigen::Vector4d(1.0, 1.5, -2.0, 0.0), Eigen::Matrix4d::Identity()); - std::vector meas = {{-3.0, -3.0}, {0.0, 0.0}, {-1.2, 1.5},{-2.0, -2.0}, {2.0, 0.0}, {-1.0, 1.0}}; + vortex::prob::Gauss4d x_est(Eigen::Vector4d(1.0, 1.5, -2.0, 0.0), Eigen::Matrix4d::Identity()); + std::vector meas = {{-3.0, -3.0}, {0.0, 0.0}, {-1.2, 1.5}, {-2.0, -2.0}, {2.0, 0.0}, {-1.0, 1.0}}; - auto dyn_model = std::make_shared>(1.0); - auto sen_model = std::make_shared>(0.5); + auto dyn_model = std::make_shared>(1.0); + auto sen_model = std::make_shared>(0.5); - auto [x_final, inside, outside, x_pred, z_pred, x_updated] = pdaf.predict_next_state( - x_est, - meas, - 1.0, - dyn_model, - sen_model, - gate_threshold, - prob_of_detection, - clutter_intensity); - std::cout << "x_final: " << x_final.mean() << std::endl; + auto [x_final, inside, outside, x_pred, z_pred, x_updated] = + pdaf.predict_next_state(x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); + std::cout << "x_final: " << x_final.mean() << std::endl; - Gnuplot gp; - gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; - gp << "set size ratio -1\n"; + Gnuplot gp; + gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; + gp << "set size ratio -1\n"; - gp << "set style circle radius 0.05\n"; - gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; - gp.send1d(meas); + gp << "set style circle radius 0.05\n"; + gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; + gp.send1d(meas); - int object_counter = 0; + int object_counter = 0; - for (const auto& state: x_updated) - { - vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2,2)); - vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); + for (const auto &state : x_updated) { + vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2, 2)); + vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); - gp << "set object " << ++object_counter - << " ellipse center " << ellipse.x << "," << ellipse.y - << " size " << ellipse.a << "," << ellipse.b - << " angle " << ellipse.angle - << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + gp << "set object " << ++object_counter << " ellipse center " << ellipse.x << "," << ellipse.y << " size " << ellipse.a << "," << ellipse.b << " angle " + << ellipse.angle << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; - gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; - } + gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; + } - gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'black'\n"; - gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'pink'\n"; - gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; + gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 + << " fs empty border lc rgb 'black'\n"; + gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 + << " fs empty border lc rgb 'pink'\n"; + gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) << " size " << 0.05 + << " fs empty border lc rgb 'green'\n"; - gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " nohead lc rgb 'pink'\n"; - gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n"; + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " nohead lc rgb 'pink'\n"; + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n"; - vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); - gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle - << " fs empty border lc rgb 'cyan'\n"; + vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); + gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle + << " fs empty border lc rgb 'cyan'\n"; - gp << "replot\n"; + gp << "replot\n"; } TEST(PDAF, predict_next_state_3_1) { - - double gate_threshold = 4; - double prob_of_detection = 0.9; - double clutter_intensity = 1.0; - SimplePDAF pdaf; - - vortex::prob::Gauss4d x_est(Eigen::Vector4d(0.5, 0.5, -0.75, -0.75), Eigen::Matrix4d::Identity()); - std::vector meas = {{0.0, 0.5}, {0.2, 0.2}, {0.8, 2.3}, {0.5, 0.0}, {4.2, 2.7}, {1.4, 2.5}}; - - auto dyn_model = std::make_shared>(0.5); - auto sen_model = std::make_shared>(1.0); - - auto [x_final, inside, outside, x_pred, z_pred, x_updated] = pdaf.predict_next_state( - x_est, - meas, - 1.0, - dyn_model, - sen_model, - gate_threshold, - prob_of_detection, - clutter_intensity); - std::cout << "x_final: " << x_final.mean() << std::endl; - - Gnuplot gp; - gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; - gp << "set size ratio -1\n"; - - gp << "set style circle radius 0.05\n"; - gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; - gp.send1d(meas); - - int object_counter = 0; - - for (const auto& state: x_updated) - { - vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2,2)); - vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); - - gp << "set object " << ++object_counter - << " ellipse center " << ellipse.x << "," << ellipse.y - << " size " << ellipse.a << "," << ellipse.b - << " angle " << ellipse.angle - << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; - - gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; - } - - gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'black'\n"; - gp << "set object " << ++object_counter << " circle center " << 0.5 << "," << 0.5 << " size " << 0.05 << " fs empty border lc rgb 'black'\n"; - gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'pink'\n"; - gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; - - gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " nohead lc rgb 'pink'\n"; - gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n"; - - vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); - gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle - << " fs empty border lc rgb 'cyan'\n"; - - gp << "replot\n"; + + double gate_threshold = 4; + double prob_of_detection = 0.9; + double clutter_intensity = 1.0; + SimplePDAF pdaf; + + vortex::prob::Gauss4d x_est(Eigen::Vector4d(0.5, 0.5, -0.75, -0.75), Eigen::Matrix4d::Identity()); + std::vector meas = {{0.0, 0.5}, {0.2, 0.2}, {0.8, 2.3}, {0.5, 0.0}, {4.2, 2.7}, {1.4, 2.5}}; + + auto dyn_model = std::make_shared>(0.5); + auto sen_model = std::make_shared>(1.0); + + auto [x_final, inside, outside, x_pred, z_pred, x_updated] = + pdaf.predict_next_state(x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); + std::cout << "x_final: " << x_final.mean() << std::endl; + + Gnuplot gp; + gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; + gp << "set size ratio -1\n"; + + gp << "set style circle radius 0.05\n"; + gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; + gp.send1d(meas); + + int object_counter = 0; + + for (const auto &state : x_updated) { + vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2, 2)); + vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); + + gp << "set object " << ++object_counter << " ellipse center " << ellipse.x << "," << ellipse.y << " size " << ellipse.a << "," << ellipse.b << " angle " + << ellipse.angle << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + + gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; + } + + gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 + << " fs empty border lc rgb 'black'\n"; + gp << "set object " << ++object_counter << " circle center " << 0.5 << "," << 0.5 << " size " << 0.05 << " fs empty border lc rgb 'black'\n"; + gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 + << " fs empty border lc rgb 'pink'\n"; + gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) << " size " << 0.05 + << " fs empty border lc rgb 'green'\n"; + + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " nohead lc rgb 'pink'\n"; + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n"; + + vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); + gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle + << " fs empty border lc rgb 'cyan'\n"; + + gp << "replot\n"; } TEST(PDAF, predict_next_state_3_2) { - - double gate_threshold = 4; - double prob_of_detection = 0.9; - double clutter_intensity = 1.0; - SimplePDAF pdaf; - - vortex::prob::Gauss4d x_est(Eigen::Vector4d(-0.00173734, 0.0766262, -0.614584, -0.57184), Eigen::Matrix4d::Identity()); - std::vector meas = {{-0.5, 2.0}, {-0.23, 0.5}, {-2.0, 3.4}, {0.0, 1.3}, {0.14, 0.5}, {-2.5, 0.89}}; - - auto dyn_model = std::make_shared>(0.5); - auto sen_model = std::make_shared>(1.0); - - auto [x_final, inside, outside, x_pred, z_pred, x_updated] = pdaf.predict_next_state( - x_est, - meas, - 1.0, - dyn_model, - sen_model, - gate_threshold, - prob_of_detection, - clutter_intensity); - std::cout << "x_final: " << x_final.mean() << std::endl; - - Gnuplot gp; - gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; - gp << "set size ratio -1\n"; - - gp << "set style circle radius 0.05\n"; - gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; - gp.send1d(meas); - - int object_counter = 0; - - for (const auto& state: x_updated) - { - vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2,2)); - vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); - - gp << "set object " << ++object_counter - << " ellipse center " << ellipse.x << "," << ellipse.y - << " size " << ellipse.a << "," << ellipse.b - << " angle " << ellipse.angle - << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; - - gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; - } - - gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'black'\n"; - gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'pink'\n"; - gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; - - gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " nohead lc rgb 'pink'\n"; - gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n"; - - // old state from 3_1 - gp << "set object " << ++object_counter << " circle center " << 0.5 << "," << 0.5 << " size " << 0.05 << " fs empty border lc rgb 'orange-red'\n"; - gp << "set arrow from " << 0.5 << "," << 0.5 << " to " << -0.00173734 << "," << 0.0766262 << " nohead lc rgb 'orange-red'\n"; - - vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); - gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle - << " fs empty border lc rgb 'cyan'\n"; - - gp << "replot\n"; + + double gate_threshold = 4; + double prob_of_detection = 0.9; + double clutter_intensity = 1.0; + SimplePDAF pdaf; + + vortex::prob::Gauss4d x_est(Eigen::Vector4d(-0.00173734, 0.0766262, -0.614584, -0.57184), Eigen::Matrix4d::Identity()); + std::vector meas = {{-0.5, 2.0}, {-0.23, 0.5}, {-2.0, 3.4}, {0.0, 1.3}, {0.14, 0.5}, {-2.5, 0.89}}; + + auto dyn_model = std::make_shared>(0.5); + auto sen_model = std::make_shared>(1.0); + + auto [x_final, inside, outside, x_pred, z_pred, x_updated] = + pdaf.predict_next_state(x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); + std::cout << "x_final: " << x_final.mean() << std::endl; + + Gnuplot gp; + gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; + gp << "set size ratio -1\n"; + + gp << "set style circle radius 0.05\n"; + gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; + gp.send1d(meas); + + int object_counter = 0; + + for (const auto &state : x_updated) { + vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2, 2)); + vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); + + gp << "set object " << ++object_counter << " ellipse center " << ellipse.x << "," << ellipse.y << " size " << ellipse.a << "," << ellipse.b << " angle " + << ellipse.angle << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + + gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; + } + + gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 + << " fs empty border lc rgb 'black'\n"; + gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 + << " fs empty border lc rgb 'pink'\n"; + gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) << " size " << 0.05 + << " fs empty border lc rgb 'green'\n"; + + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " nohead lc rgb 'pink'\n"; + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n"; + + // old state from 3_1 + gp << "set object " << ++object_counter << " circle center " << 0.5 << "," << 0.5 << " size " << 0.05 << " fs empty border lc rgb 'orange-red'\n"; + gp << "set arrow from " << 0.5 << "," << 0.5 << " to " << -0.00173734 << "," << 0.0766262 << " nohead lc rgb 'orange-red'\n"; + + vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); + gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle + << " fs empty border lc rgb 'cyan'\n"; + + gp << "replot\n"; } TEST(PDAF, predict_next_state_3_3) { - - double gate_threshold = 4; - double prob_of_detection = 0.9; - double clutter_intensity = 1.0; - SimplePDAF pdaf; - - vortex::prob::Gauss4d x_est(Eigen::Vector4d(-0.55929, 0.0694888, -0.583476, -0.26382), Eigen::Matrix4d::Identity()); - std::vector meas = {{-1.5, 2.5}, {-1.2, 2.7}, {-0.8, 2.3},{-1.7, 1.9}, {-2.0, 3.0}, {-1.11, 2.04}}; - - auto dyn_model = std::make_shared>(0.5); - auto sen_model = std::make_shared>(1.0); - - auto [x_final, inside, outside, x_pred, z_pred, x_updated] = pdaf.predict_next_state( - x_est, - meas, - 1.0, - dyn_model, - sen_model, - gate_threshold, - prob_of_detection, - clutter_intensity); - std::cout << "x_final: " << x_final.mean() << std::endl; - - Gnuplot gp; - gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; - gp << "set size ratio -1\n"; - - gp << "set style circle radius 0.05\n"; - gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; - gp.send1d(meas); - - int object_counter = 0; - - for (const auto& state: x_updated) - { - vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2,2)); - vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); - - gp << "set object " << ++object_counter - << " ellipse center " << ellipse.x << "," << ellipse.y - << " size " << ellipse.a << "," << ellipse.b - << " angle " << ellipse.angle - << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; - - gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; - } - - gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'black'\n"; - gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'pink'\n"; - gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; - - gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " nohead lc rgb 'pink'\n"; - gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n"; - - // old state from 3_1, 3_2 - gp << "set object " << ++object_counter << " circle center " << 0.5 << "," << 0.5 << " size " << 0.05 << " fs empty border lc rgb 'orange-red'\n"; - gp << "set arrow from " << 0.5 << "," << 0.5 << " to " << -0.00173734 << "," << 0.0766262 << " nohead lc rgb 'orange-red'\n"; - gp << "set object " << ++object_counter << " circle center " << -0.00173734 << "," << 0.0766262 << " size " << 0.05 << " fs empty border lc rgb 'orange-red'\n"; - gp << "set arrow from " << -0.00173734 << "," << 0.0766262 << " to " << -0.55929 << "," << 0.0694888 << " nohead lc rgb 'orange-red'\n"; - - vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); - gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle - << " fs empty border lc rgb 'cyan'\n"; - - gp << "replot\n"; + + double gate_threshold = 4; + double prob_of_detection = 0.9; + double clutter_intensity = 1.0; + SimplePDAF pdaf; + + vortex::prob::Gauss4d x_est(Eigen::Vector4d(-0.55929, 0.0694888, -0.583476, -0.26382), Eigen::Matrix4d::Identity()); + std::vector meas = {{-1.5, 2.5}, {-1.2, 2.7}, {-0.8, 2.3}, {-1.7, 1.9}, {-2.0, 3.0}, {-1.11, 2.04}}; + + auto dyn_model = std::make_shared>(0.5); + auto sen_model = std::make_shared>(1.0); + + auto [x_final, inside, outside, x_pred, z_pred, x_updated] = + pdaf.predict_next_state(x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); + std::cout << "x_final: " << x_final.mean() << std::endl; + + Gnuplot gp; + gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; + gp << "set size ratio -1\n"; + + gp << "set style circle radius 0.05\n"; + gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; + gp.send1d(meas); + + int object_counter = 0; + + for (const auto &state : x_updated) { + vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2, 2)); + vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); + + gp << "set object " << ++object_counter << " ellipse center " << ellipse.x << "," << ellipse.y << " size " << ellipse.a << "," << ellipse.b << " angle " + << ellipse.angle << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + + gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; + } + + gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 + << " fs empty border lc rgb 'black'\n"; + gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 + << " fs empty border lc rgb 'pink'\n"; + gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) << " size " << 0.05 + << " fs empty border lc rgb 'green'\n"; + + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " nohead lc rgb 'pink'\n"; + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n"; + + // old state from 3_1, 3_2 + gp << "set object " << ++object_counter << " circle center " << 0.5 << "," << 0.5 << " size " << 0.05 << " fs empty border lc rgb 'orange-red'\n"; + gp << "set arrow from " << 0.5 << "," << 0.5 << " to " << -0.00173734 << "," << 0.0766262 << " nohead lc rgb 'orange-red'\n"; + gp << "set object " << ++object_counter << " circle center " << -0.00173734 << "," << 0.0766262 << " size " << 0.05 + << " fs empty border lc rgb 'orange-red'\n"; + gp << "set arrow from " << -0.00173734 << "," << 0.0766262 << " to " << -0.55929 << "," << 0.0694888 << " nohead lc rgb 'orange-red'\n"; + + vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); + gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle + << " fs empty border lc rgb 'cyan'\n"; + + gp << "replot\n"; } TEST(PDAF, predict_next_state_3_4) { - - double gate_threshold = 4; - double prob_of_detection = 0.9; - double clutter_intensity = 1.0; - SimplePDAF pdaf; - - vortex::prob::Gauss4d x_est(Eigen::Vector4d(-1.20613, 0.610616, -0.618037, 0.175242), Eigen::Matrix4d::Identity()); - std::vector meas = {{-2.0, 2.2}, {-1.8, 2.3}, {-2.3, 2.0}, {0.6, 1.5}, {-2.0, 2.0}, {-1.4, 2.5}}; - - auto dyn_model = std::make_shared>(0.5); - auto sen_model = std::make_shared>(1.0); - - auto [x_final, inside, outside, x_pred, z_pred, x_updated] = pdaf.predict_next_state( - x_est, - meas, - 1.0, - dyn_model, - sen_model, - gate_threshold, - prob_of_detection, - clutter_intensity); - std::cout << "x_final: " << x_final.mean() << std::endl; - - Gnuplot gp; - gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; - gp << "set size ratio -1\n"; - - gp << "set style circle radius 0.05\n"; - gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; - gp.send1d(meas); - - int object_counter = 0; - - for (const auto& state: x_updated) - { - vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2,2)); - vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); - - gp << "set object " << ++object_counter - << " ellipse center " << ellipse.x << "," << ellipse.y - << " size " << ellipse.a << "," << ellipse.b - << " angle " << ellipse.angle - << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; - - gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; - } - - gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'black'\n"; - gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'pink'\n"; - gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; - - gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " nohead lc rgb 'pink'\n"; - gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n"; - - // old state from 3_1, 3_2, 3_3 - gp << "set object " << ++object_counter << " circle center " << 0.5 << "," << 0.5 << " size " << 0.05 << " fs empty border lc rgb 'orange-red'\n"; - gp << "set arrow from " << 0.5 << "," << 0.5 << " to " << -0.00173734 << "," << 0.0766262 << " nohead lc rgb 'orange-red'\n"; - gp << "set object " << ++object_counter << " circle center " << -0.00173734 << "," << 0.0766262 << " size " << 0.05 << " fs empty border lc rgb 'orange-red'\n"; - gp << "set arrow from " << -0.00173734 << "," << 0.0766262 << " to " << -0.55929 << "," << 0.0694888 << " nohead lc rgb 'orange-red'\n"; - gp << "set object " << ++object_counter << " circle center " << -0.55929 << "," << 0.0694888 << " size " << 0.05 << " fs empty border lc rgb 'orange-red'\n"; - gp << "set arrow from " << -0.55929 << "," << 0.0694888 << " to " << -1.20613 << "," << 0.610616 << " nohead lc rgb 'orange-red'\n"; - - vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); - gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle - << " fs empty border lc rgb 'cyan'\n"; - - gp << "replot\n"; + + double gate_threshold = 4; + double prob_of_detection = 0.9; + double clutter_intensity = 1.0; + SimplePDAF pdaf; + + vortex::prob::Gauss4d x_est(Eigen::Vector4d(-1.20613, 0.610616, -0.618037, 0.175242), Eigen::Matrix4d::Identity()); + std::vector meas = {{-2.0, 2.2}, {-1.8, 2.3}, {-2.3, 2.0}, {0.6, 1.5}, {-2.0, 2.0}, {-1.4, 2.5}}; + + auto dyn_model = std::make_shared>(0.5); + auto sen_model = std::make_shared>(1.0); + + auto [x_final, inside, outside, x_pred, z_pred, x_updated] = + pdaf.predict_next_state(x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); + std::cout << "x_final: " << x_final.mean() << std::endl; + + Gnuplot gp; + gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; + gp << "set size ratio -1\n"; + + gp << "set style circle radius 0.05\n"; + gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; + gp.send1d(meas); + + int object_counter = 0; + + for (const auto &state : x_updated) { + vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2, 2)); + vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); + + gp << "set object " << ++object_counter << " ellipse center " << ellipse.x << "," << ellipse.y << " size " << ellipse.a << "," << ellipse.b << " angle " + << ellipse.angle << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + + gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; + } + + gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 + << " fs empty border lc rgb 'black'\n"; + gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 + << " fs empty border lc rgb 'pink'\n"; + gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) << " size " << 0.05 + << " fs empty border lc rgb 'green'\n"; + + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " nohead lc rgb 'pink'\n"; + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n"; + + // old state from 3_1, 3_2, 3_3 + gp << "set object " << ++object_counter << " circle center " << 0.5 << "," << 0.5 << " size " << 0.05 << " fs empty border lc rgb 'orange-red'\n"; + gp << "set arrow from " << 0.5 << "," << 0.5 << " to " << -0.00173734 << "," << 0.0766262 << " nohead lc rgb 'orange-red'\n"; + gp << "set object " << ++object_counter << " circle center " << -0.00173734 << "," << 0.0766262 << " size " << 0.05 + << " fs empty border lc rgb 'orange-red'\n"; + gp << "set arrow from " << -0.00173734 << "," << 0.0766262 << " to " << -0.55929 << "," << 0.0694888 << " nohead lc rgb 'orange-red'\n"; + gp << "set object " << ++object_counter << " circle center " << -0.55929 << "," << 0.0694888 << " size " << 0.05 << " fs empty border lc rgb 'orange-red'\n"; + gp << "set arrow from " << -0.55929 << "," << 0.0694888 << " to " << -1.20613 << "," << 0.610616 << " nohead lc rgb 'orange-red'\n"; + + vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); + gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle + << " fs empty border lc rgb 'cyan'\n"; + + gp << "replot\n"; } \ No newline at end of file From f86ea0bc7a5abf12aeaaeeb2b12e77708299747a Mon Sep 17 00:00:00 2001 From: Sondre Selberg Date: Tue, 30 Jan 2024 20:15:48 +0100 Subject: [PATCH 12/31] "da er ipda ferdig" -Eirik --- .../include/vortex_filtering/filters/ipda.hpp | 130 +++++++++++++----- .../probability/multi_var_gauss.hpp | 2 +- 2 files changed, 100 insertions(+), 32 deletions(-) diff --git a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp index 1ac5978e..a5dfd8e5 100644 --- a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp @@ -1,44 +1,112 @@ #ifndef IPDA_HPP #define IPDA_HPP -#include +#pragma once #include +#include +#include +#include +#include -class IPDAFilter { +namespace vortex::filter { +template +class IPDA { public: - IPDAFilter(); - double generate_new_probability(std::vector &measurements, double &probability_of_survival, float &probability_of_detection, - double &standard_deviation, float &lambda); - -private: - double last_detection_probability_; - double get_last_detection_probability(); - double sum_of_gaussian_probabilities(std::vector &measurements, double &standard_deviation); -}; + using SensModI = typename SensModT::SensModI; + using DynModI = typename DynModT::DynModI; + using DynModPtr = std::shared_ptr; + using SensModPtr = std::shared_ptr; + using EKF = vortex::filter::EKF; + using Gauss_z = typename SensModI::Gauss_z; + using Gauss_x = typename DynModI::Gauss_x; + using Vec_z = typename SensModI::Vec_z; + using GaussMix = vortex::prob::GaussianMixture; + using FAKE_PDAF = vortex::filter::PDAF; + IPDA(){ + + } + /// @brief + /// @param measurements Measurements to iterate over + /// @param probability_of_survival How likely the object is to survive (Ps) + /// @param probability_of_detection How likely the object is to be detected (Pd) + /// @param standard_deviation Standard deviation of the measurements (Sk) + /// @param lambda Lambda value for the Poisson distribution (Lambda) + static double get_existence_probability(const std::vector &measurements, double probability_of_survival, double last_detection_probability_, double probability_of_detection, double clutter_intensity, Gauss_z &z_pred){ + double predicted_existence_probability = probability_of_survival * last_detection_probability_; // Finds Ps -IPDAFilter::IPDAFilter() { last_detection_probability_ = 0.0; } + double summed = 0; + for(const Vec_z &measurement : measurements){ + summed += z_pred.pfd(measurement); + } + double l_k = 1 - probability_of_detection + probability_of_detection / clutter_intensity * summed; -/// @brief -/// @param measurements Measurements to iterate over -/// @param probability_of_survival How likely the object is to survive (Ps) -/// @param probability_of_detection How likely the object is to be detected (Pd) -/// @param standard_deviation Standard deviation of the measurements (Sk) -/// @param lambda Lambda value for the Poisson distribution (Lambda) -double IPDAFilter::generate_new_probability(std::vector &measurements, double &probability_of_survival, float &probability_of_detection, - double &standard_deviation, float &lambda) -{ - double predicted_existence_probability = probability_of_survival * last_detection_probability_; - double l_k = 1 - probability_of_detection + probability_of_detection / lambda * sum_of_gaussian_probabilities(measurements, standard_deviation); + return (l_k * predicted_existence_probability) / (1 - (1 - l_k) * predicted_existence_probability); + } + static std::tuple, std::vector, Gauss_x, Gauss_z, std::vector> step( + const Gauss_x& x_est, + const std::vector& z_meas, + double timestep, + const DynModPtr& dyn_model, + const SensModPtr& sen_model, + double gate_threshold, + double prob_of_detection, + double prob_of_survival, + double survive_est, + double clutter_intensity + ) const + { + auto [x_pred, z_pred] = EKF::predict(dyn_model, sen_model, timestep, x_est); + auto [inside, outside] = apply_gate(z_meas, z_pred, gate_threshold); - return (l_k * predicted_existence_probability) / (1 - (1 - l_k) * predicted_existence_probability); -} -double IPDAFilter::sum_of_gaussian_probabilities(std::vector &measurements, double &standard_deviation) -{ - double sum = 0.0; + std::vector x_updated; + for (const auto& measurement : inside) { + x_updated.push_back(EKF::update(sen_model, x_pred, z_pred, measurement)); + } - return sum; -} + Gauss_x x_final = get_weighted_average( + z_meas, + x_updated, + z_pred, + x_pred, + prob_of_detection, + clutter_intensity); -double IPDAFilter::get_last_detection_probability() { return last_detection_probability_; } + double existence_probability = get_existence_probability(z_meas, prob_of_survival, survive_est, prob_of_detection, clutter_intensity, z_pred); + return {x_final, existence_probability, inside, outside, x_pred, z_pred, x_updated}; + } + static std::tuple, std::vector> apply_gate( + const std::vector& z_meas, + const Gauss_z& z_pred, + double gate_threshold + ) { + FAKE_PDAF pdaf; + return pdaf.apply_gate(z_meas, z_pred, gate_threshold); + } + static Gauss_x get_weighted_average( + const std::vector& z_meas, + const std::vector& updated_states, + const Gauss_z& z_pred, + const Gauss_x& x_pred, + double prob_of_detection, + double clutter_intensity + ){ + FAKE_PDAF pdaf; + return pdaf.get_weighted_average(z_meas, updated_states, z_pred, x_pred, prob_of_detection, clutter_intensity); + } + Eigen::VectorXd get_weights( + const std::vector& z_meas, + const Gauss_z& z_pred, + double prob_of_detection, + double clutter_intensity + ){ + FAKE_PDAF pdaf; + return pdaf.get_weights(z_meas, z_pred, prob_of_detection, clutter_intensity); + } +}; +} // namespace vortex::filter #endif // IPDA_HPP + + + + diff --git a/vortex-filtering/include/vortex_filtering/probability/multi_var_gauss.hpp b/vortex-filtering/include/vortex_filtering/probability/multi_var_gauss.hpp index 17adb2cb..43349e54 100644 --- a/vortex-filtering/include/vortex_filtering/probability/multi_var_gauss.hpp +++ b/vortex-filtering/include/vortex_filtering/probability/multi_var_gauss.hpp @@ -32,7 +32,7 @@ template class MultiVarGauss { // Default constructor MultiVarGauss() = default; - /** Calculate the probability density function of x given the Gaussian + /** Calculate the probability density of x given the Gaussian * @param x * @return double */ From 1f56997463ae169c9922de83df57de38d13f8027 Mon Sep 17 00:00:00 2001 From: TristanWolfram Date: Sun, 4 Feb 2024 14:55:48 +0100 Subject: [PATCH 13/31] =?UTF-8?q?=F0=9F=9A=80=20reformat=20code?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- vortex-filtering/test/pdaf_test.cpp | 363 ++++++++++++++++------------ 1 file changed, 209 insertions(+), 154 deletions(-) diff --git a/vortex-filtering/test/pdaf_test.cpp b/vortex-filtering/test/pdaf_test.cpp index 243ad81f..d0c26f5e 100644 --- a/vortex-filtering/test/pdaf_test.cpp +++ b/vortex-filtering/test/pdaf_test.cpp @@ -6,7 +6,10 @@ using SimplePDAF = vortex::filter::PDAF, vortex::models::IdentitySensorModel<4, 2>>; -TEST(PDAF, init) { SimplePDAF pdaf; } +TEST(PDAF, init) +{ + SimplePDAF pdaf; +} // testing the get_weights function TEST(PDAF, get_weights_is_calculating) @@ -17,7 +20,7 @@ TEST(PDAF, get_weights_is_calculating) double clutter_intensity = 1.0; vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); - std::vector meas = {{0.0, 0.0}, {2.0, 1.0}}; + std::vector meas = { { 0.0, 0.0 }, { 2.0, 1.0 } }; Eigen::VectorXd weights = pdaf.get_weights(meas, z_pred, prob_of_detection, clutter_intensity); @@ -34,7 +37,7 @@ TEST(PDAF, if_no_clutter_first_weight_is_zero) double clutter_intensity = 0.0; vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); - std::vector meas = {{0.0, 0.0}, {2.0, 1.0}}; + std::vector meas = { { 0.0, 0.0 }, { 2.0, 1.0 } }; Eigen::VectorXd weights = pdaf.get_weights(meas, z_pred, prob_of_detection, clutter_intensity); @@ -51,7 +54,7 @@ TEST(PDAF, weights_are_decreasing_with_distance) double clutter_intensity = 1.0; vortex::prob::Gauss2d z_pred(Eigen::Vector2d(1.0, 1.0), Eigen::Matrix2d::Identity()); - std::vector meas = {{2.0, 1.0}, {3.0, 1.0}, {4.0, 1.0}}; + std::vector meas = { { 2.0, 1.0 }, { 3.0, 1.0 }, { 4.0, 1.0 } }; Eigen::VectorXd weights = pdaf.get_weights(meas, z_pred, prob_of_detection, clutter_intensity); @@ -71,11 +74,14 @@ TEST(PDAF, get_weighted_average_is_calculating) vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); vortex::prob::Gauss4d x_pred(Eigen::Vector4d(0.0, 0.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); - std::vector meas = {{0.0, 0.0}, {2.0, 1.0}}; - std::vector updated_states = {vortex::prob::Gauss4d(Eigen::Vector4d(0.0, 0.0, 0.0, 0.0), Eigen::Matrix4d::Identity()), - vortex::prob::Gauss4d(Eigen::Vector4d(1.0, 1.0, 1.0, 1.0), Eigen::Matrix4d::Identity())}; + std::vector meas = { { 0.0, 0.0 }, { 2.0, 1.0 } }; + std::vector updated_states = { + vortex::prob::Gauss4d(Eigen::Vector4d(0.0, 0.0, 0.0, 0.0), Eigen::Matrix4d::Identity()), + vortex::prob::Gauss4d(Eigen::Vector4d(1.0, 1.0, 1.0, 1.0), Eigen::Matrix4d::Identity()) + }; - vortex::prob::Gauss4d weighted_average = pdaf.get_weighted_average(meas, updated_states, z_pred, x_pred, prob_of_detection, clutter_intensity); + vortex::prob::Gauss4d weighted_average = + pdaf.get_weighted_average(meas, updated_states, z_pred, x_pred, prob_of_detection, clutter_intensity); std::cout << "weighted average: " << weighted_average.mean() << std::endl; } @@ -89,10 +95,12 @@ TEST(PDAF, average_state_is_in_between_prediction_and_measurement_y_axis) vortex::prob::Gauss2d z_pred(Eigen::Vector2d(1.0, 1.0), Eigen::Matrix2d::Identity()); vortex::prob::Gauss4d x_pred(Eigen::Vector4d(1.0, 1.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); - std::vector meas = {{1.0, 2.0}}; - std::vector updated_states = {vortex::prob::Gauss4d(Eigen::Vector4d(1.0, 1.5, 0.0, 0.0), Eigen::Matrix4d::Identity())}; + std::vector meas = { { 1.0, 2.0 } }; + std::vector updated_states = { vortex::prob::Gauss4d(Eigen::Vector4d(1.0, 1.5, 0.0, 0.0), + Eigen::Matrix4d::Identity()) }; - vortex::prob::Gauss4d weighted_average = pdaf.get_weighted_average(meas, updated_states, z_pred, x_pred, prob_of_detection, clutter_intensity); + vortex::prob::Gauss4d weighted_average = + pdaf.get_weighted_average(meas, updated_states, z_pred, x_pred, prob_of_detection, clutter_intensity); EXPECT_GT(weighted_average.mean()(1), x_pred.mean()(1)); EXPECT_GT(weighted_average.mean()(1), z_pred.mean()(1)); @@ -111,10 +119,12 @@ TEST(PDAF, average_state_is_in_between_prediction_and_measurement_x_axis) vortex::prob::Gauss2d z_pred(Eigen::Vector2d(1.0, 1.0), Eigen::Matrix2d::Identity()); vortex::prob::Gauss4d x_pred(Eigen::Vector4d(1.0, 1.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); - std::vector meas = {{2.0, 1.0}}; - std::vector updated_states = {vortex::prob::Gauss4d(Eigen::Vector4d(1.5, 1.0, 0.0, 0.0), Eigen::Matrix4d::Identity())}; + std::vector meas = { { 2.0, 1.0 } }; + std::vector updated_states = { vortex::prob::Gauss4d(Eigen::Vector4d(1.5, 1.0, 0.0, 0.0), + Eigen::Matrix4d::Identity()) }; - vortex::prob::Gauss4d weighted_average = pdaf.get_weighted_average(meas, updated_states, z_pred, x_pred, prob_of_detection, clutter_intensity); + vortex::prob::Gauss4d weighted_average = + pdaf.get_weighted_average(meas, updated_states, z_pred, x_pred, prob_of_detection, clutter_intensity); EXPECT_GT(weighted_average.mean()(0), x_pred.mean()(0)); EXPECT_GT(weighted_average.mean()(0), z_pred.mean()(0)); @@ -133,10 +143,12 @@ TEST(PDAF, average_state_is_in_between_prediction_and_measurement_both_axes) vortex::prob::Gauss2d z_pred(Eigen::Vector2d(1.0, 1.0), Eigen::Matrix2d::Identity()); vortex::prob::Gauss4d x_pred(Eigen::Vector4d(1.0, 1.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); - std::vector meas = {{2.0, 2.0}}; - std::vector updated_states = {vortex::prob::Gauss4d(Eigen::Vector4d(1.5, 1.5, 0.0, 0.0), Eigen::Matrix4d::Identity())}; + std::vector meas = { { 2.0, 2.0 } }; + std::vector updated_states = { vortex::prob::Gauss4d(Eigen::Vector4d(1.5, 1.5, 0.0, 0.0), + Eigen::Matrix4d::Identity()) }; - vortex::prob::Gauss4d weighted_average = pdaf.get_weighted_average(meas, updated_states, z_pred, x_pred, prob_of_detection, clutter_intensity); + vortex::prob::Gauss4d weighted_average = + pdaf.get_weighted_average(meas, updated_states, z_pred, x_pred, prob_of_detection, clutter_intensity); EXPECT_GT(weighted_average.mean()(0), x_pred.mean()(0)); EXPECT_GT(weighted_average.mean()(0), z_pred.mean()(0)); @@ -154,13 +166,13 @@ TEST(PDAF, average_state_is_in_between_prediction_and_measurement_both_axes) // testing the apply_gate function TEST(PDAF, apply_gate_is_calculating) { - SimplePDAF pdaf; double gate_threshold = 1.8; vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); - std::vector meas = {{0.0, 1.0}, {1.0, 0.0}, {1.0, 1.0}, {0.0, 2.0}, {2.0, 0.0}, {2.0, 2.0}}; + std::vector meas = { { 0.0, 1.0 }, { 1.0, 0.0 }, { 1.0, 1.0 }, + { 0.0, 2.0 }, { 2.0, 0.0 }, { 2.0, 2.0 } }; auto [inside, outside] = pdaf.apply_gate(meas, z_pred, gate_threshold); } @@ -174,7 +186,7 @@ TEST(PDAF, apply_gate_is_separating_correctly) cov << 1.0, 0.0, 0.0, 4.0; vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), cov); - std::vector meas = {{0.0, 4.0}, {4.0, 0.0}}; + std::vector meas = { { 0.0, 4.0 }, { 4.0, 0.0 } }; auto [inside, outside] = pdaf.apply_gate(meas, z_pred, gate_threshold); @@ -192,14 +204,14 @@ TEST(PDAF, apply_gate_is_separating_correctly) int object_counter = 0; - gp << "set object " << ++object_counter << " circle center " << z_pred.mean()(0) << "," << z_pred.mean()(1) << " size " << 0.05 - << " fs empty border lc rgb 'green'\n"; + gp << "set object " << ++object_counter << " circle center " << z_pred.mean()(0) << "," << z_pred.mean()(1) + << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; gp << "replot\n"; vortex::plotting::Ellipse prediction = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); - gp << "set object " << ++object_counter << " ellipse center " << prediction.x << "," << prediction.y << " size " << prediction.a << "," << prediction.b - << " angle " << prediction.angle << "fs empty border lc rgb 'cyan'\n"; + gp << "set object " << ++object_counter << " ellipse center " << prediction.x << "," << prediction.y << " size " + << prediction.a << "," << prediction.b << " angle " << prediction.angle << "fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; } @@ -209,7 +221,8 @@ TEST(PDAF, apply_gate_is_separating_correctly_2) SimplePDAF pdaf; vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); - std::vector meas = {{0.0, 1.0}, {1.0, 0.0}, {1.0, 1.0}, {0.0, 2.0}, {2.0, 0.0}, {2.0, 2.0}}; + std::vector meas = { { 0.0, 1.0 }, { 1.0, 0.0 }, { 1.0, 1.0 }, + { 0.0, 2.0 }, { 2.0, 0.0 }, { 2.0, 2.0 } }; auto [inside, outside] = pdaf.apply_gate(meas, z_pred, gate_threshold); @@ -225,34 +238,34 @@ TEST(PDAF, apply_gate_is_separating_correctly_2) int object_counter = 0; - gp << "set object " << ++object_counter << " circle center " << z_pred.mean()(0) << "," << z_pred.mean()(1) << " size " << 0.05 - << " fs empty border lc rgb 'green'\n"; + gp << "set object " << ++object_counter << " circle center " << z_pred.mean()(0) << "," << z_pred.mean()(1) + << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; gp << "replot\n"; vortex::plotting::Ellipse prediction = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); - gp << "set object " << ++object_counter << " ellipse center " << prediction.x << "," << prediction.y << " size " << prediction.a << "," << prediction.b - << " angle " << prediction.angle << "fs empty border lc rgb 'cyan'\n"; + gp << "set object " << ++object_counter << " ellipse center " << prediction.x << "," << prediction.y << " size " + << prediction.a << "," << prediction.b << " angle " << prediction.angle << "fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; } // testing the predict_next_state function TEST(PDAF, predict_next_state_is_calculating) { - - double gate_threshold = 1.12; + double gate_threshold = 1.12; double prob_of_detection = 0.8; double clutter_intensity = 1.0; SimplePDAF pdaf; vortex::prob::Gauss4d x_est(Eigen::Vector4d(0.0, 0.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); - std::vector meas = {{0.0, 1.0}, {1.0, 0.0}, {1.0, 1.0}, {0.0, 2.0}, {2.0, 0.0}, {2.0, 2.0}}; + std::vector meas = { { 0.0, 1.0 }, { 1.0, 0.0 }, { 1.0, 1.0 }, + { 0.0, 2.0 }, { 2.0, 0.0 }, { 2.0, 2.0 } }; auto dyn_model = std::make_shared>(1.0); auto sen_model = std::make_shared>(1.0); - auto [x_final, inside, outside, x_pred, z_pred, x_updated] = - pdaf.predict_next_state(x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); + auto [x_final, inside, outside, x_pred, z_pred, x_updated] = pdaf.predict_next_state( + x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); std::cout << "x_final: " << x_final.mean() << std::endl; Gnuplot gp; @@ -265,49 +278,54 @@ TEST(PDAF, predict_next_state_is_calculating) int object_counter = 0; - for (const auto &state : x_updated) { + for (const auto& state : x_updated) + { vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2, 2)); vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); - gp << "set object " << ++object_counter << " ellipse center " << ellipse.x << "," << ellipse.y << " size " << ellipse.a << "," << ellipse.b << " angle " - << ellipse.angle << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + gp << "set object " << ++object_counter << " ellipse center " << ellipse.x << "," << ellipse.y << " size " + << ellipse.a << "," << ellipse.b << " angle " << ellipse.angle + << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; - gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; + gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 + << " fs empty border lc rgb 'blue'\n"; } - gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 - << " fs empty border lc rgb 'black'\n"; - gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 - << " fs empty border lc rgb 'pink'\n"; - gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) << " size " << 0.05 - << " fs empty border lc rgb 'green'\n"; + gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " + << 0.05 << " fs empty border lc rgb 'black'\n"; + gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) + << " size " << 0.05 << " fs empty border lc rgb 'pink'\n"; + gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) + << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; - gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " nohead lc rgb 'pink'\n"; - gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n"; + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," + << x_pred.mean()(1) << " nohead lc rgb 'pink'\n"; + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," + << x_final.mean()(1) << " nohead lc rgb 'green'\n"; vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); - gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle - << " fs empty border lc rgb 'cyan'\n"; + gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," + << gate.b << " angle " << gate.angle << " fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; } TEST(PDAF, predict_next_state_2) { - - double gate_threshold = 2; + double gate_threshold = 2; double prob_of_detection = 0.8; double clutter_intensity = 1.0; SimplePDAF pdaf; vortex::prob::Gauss4d x_est(Eigen::Vector4d(1.0, 1.5, -2.0, 0.0), Eigen::Matrix4d::Identity()); - std::vector meas = {{-3.0, -3.0}, {0.0, 0.0}, {-1.2, 1.5}, {-2.0, -2.0}, {2.0, 0.0}, {-1.0, 1.0}}; + std::vector meas = { { -3.0, -3.0 }, { 0.0, 0.0 }, { -1.2, 1.5 }, + { -2.0, -2.0 }, { 2.0, 0.0 }, { -1.0, 1.0 } }; auto dyn_model = std::make_shared>(1.0); auto sen_model = std::make_shared>(0.5); - auto [x_final, inside, outside, x_pred, z_pred, x_updated] = - pdaf.predict_next_state(x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); + auto [x_final, inside, outside, x_pred, z_pred, x_updated] = pdaf.predict_next_state( + x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); std::cout << "x_final: " << x_final.mean() << std::endl; Gnuplot gp; @@ -320,49 +338,54 @@ TEST(PDAF, predict_next_state_2) int object_counter = 0; - for (const auto &state : x_updated) { + for (const auto& state : x_updated) + { vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2, 2)); vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); - gp << "set object " << ++object_counter << " ellipse center " << ellipse.x << "," << ellipse.y << " size " << ellipse.a << "," << ellipse.b << " angle " - << ellipse.angle << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + gp << "set object " << ++object_counter << " ellipse center " << ellipse.x << "," << ellipse.y << " size " + << ellipse.a << "," << ellipse.b << " angle " << ellipse.angle + << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; - gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; + gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 + << " fs empty border lc rgb 'blue'\n"; } - gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 - << " fs empty border lc rgb 'black'\n"; - gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 - << " fs empty border lc rgb 'pink'\n"; - gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) << " size " << 0.05 - << " fs empty border lc rgb 'green'\n"; + gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " + << 0.05 << " fs empty border lc rgb 'black'\n"; + gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) + << " size " << 0.05 << " fs empty border lc rgb 'pink'\n"; + gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) + << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; - gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " nohead lc rgb 'pink'\n"; - gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n"; + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," + << x_pred.mean()(1) << " nohead lc rgb 'pink'\n"; + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," + << x_final.mean()(1) << " nohead lc rgb 'green'\n"; vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); - gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle - << " fs empty border lc rgb 'cyan'\n"; + gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," + << gate.b << " angle " << gate.angle << " fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; } TEST(PDAF, predict_next_state_3_1) { - - double gate_threshold = 4; + double gate_threshold = 4; double prob_of_detection = 0.9; double clutter_intensity = 1.0; SimplePDAF pdaf; vortex::prob::Gauss4d x_est(Eigen::Vector4d(0.5, 0.5, -0.75, -0.75), Eigen::Matrix4d::Identity()); - std::vector meas = {{0.0, 0.5}, {0.2, 0.2}, {0.8, 2.3}, {0.5, 0.0}, {4.2, 2.7}, {1.4, 2.5}}; + std::vector meas = { { 0.0, 0.5 }, { 0.2, 0.2 }, { 0.8, 2.3 }, + { 0.5, 0.0 }, { 4.2, 2.7 }, { 1.4, 2.5 } }; auto dyn_model = std::make_shared>(0.5); auto sen_model = std::make_shared>(1.0); - auto [x_final, inside, outside, x_pred, z_pred, x_updated] = - pdaf.predict_next_state(x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); + auto [x_final, inside, outside, x_pred, z_pred, x_updated] = pdaf.predict_next_state( + x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); std::cout << "x_final: " << x_final.mean() << std::endl; Gnuplot gp; @@ -375,50 +398,57 @@ TEST(PDAF, predict_next_state_3_1) int object_counter = 0; - for (const auto &state : x_updated) { + for (const auto& state : x_updated) + { vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2, 2)); vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); - gp << "set object " << ++object_counter << " ellipse center " << ellipse.x << "," << ellipse.y << " size " << ellipse.a << "," << ellipse.b << " angle " - << ellipse.angle << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + gp << "set object " << ++object_counter << " ellipse center " << ellipse.x << "," << ellipse.y << " size " + << ellipse.a << "," << ellipse.b << " angle " << ellipse.angle + << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; - gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; + gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 + << " fs empty border lc rgb 'blue'\n"; } - gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 + gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " + << 0.05 << " fs empty border lc rgb 'black'\n"; + gp << "set object " << ++object_counter << " circle center " << 0.5 << "," << 0.5 << " size " << 0.05 << " fs empty border lc rgb 'black'\n"; - gp << "set object " << ++object_counter << " circle center " << 0.5 << "," << 0.5 << " size " << 0.05 << " fs empty border lc rgb 'black'\n"; - gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 - << " fs empty border lc rgb 'pink'\n"; - gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) << " size " << 0.05 - << " fs empty border lc rgb 'green'\n"; + gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) + << " size " << 0.05 << " fs empty border lc rgb 'pink'\n"; + gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) + << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; - gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " nohead lc rgb 'pink'\n"; - gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n"; + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," + << x_pred.mean()(1) << " nohead lc rgb 'pink'\n"; + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," + << x_final.mean()(1) << " nohead lc rgb 'green'\n"; vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); - gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle - << " fs empty border lc rgb 'cyan'\n"; + gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," + << gate.b << " angle " << gate.angle << " fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; } TEST(PDAF, predict_next_state_3_2) { - - double gate_threshold = 4; + double gate_threshold = 4; double prob_of_detection = 0.9; double clutter_intensity = 1.0; SimplePDAF pdaf; - vortex::prob::Gauss4d x_est(Eigen::Vector4d(-0.00173734, 0.0766262, -0.614584, -0.57184), Eigen::Matrix4d::Identity()); - std::vector meas = {{-0.5, 2.0}, {-0.23, 0.5}, {-2.0, 3.4}, {0.0, 1.3}, {0.14, 0.5}, {-2.5, 0.89}}; + vortex::prob::Gauss4d x_est(Eigen::Vector4d(-0.00173734, 0.0766262, -0.614584, -0.57184), + Eigen::Matrix4d::Identity()); + std::vector meas = { { -0.5, 2.0 }, { -0.23, 0.5 }, { -2.0, 3.4 }, + { 0.0, 1.3 }, { 0.14, 0.5 }, { -2.5, 0.89 } }; auto dyn_model = std::make_shared>(0.5); auto sen_model = std::make_shared>(1.0); - auto [x_final, inside, outside, x_pred, z_pred, x_updated] = - pdaf.predict_next_state(x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); + auto [x_final, inside, outside, x_pred, z_pred, x_updated] = pdaf.predict_next_state( + x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); std::cout << "x_final: " << x_final.mean() << std::endl; Gnuplot gp; @@ -431,53 +461,60 @@ TEST(PDAF, predict_next_state_3_2) int object_counter = 0; - for (const auto &state : x_updated) { + for (const auto& state : x_updated) + { vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2, 2)); vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); - gp << "set object " << ++object_counter << " ellipse center " << ellipse.x << "," << ellipse.y << " size " << ellipse.a << "," << ellipse.b << " angle " - << ellipse.angle << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + gp << "set object " << ++object_counter << " ellipse center " << ellipse.x << "," << ellipse.y << " size " + << ellipse.a << "," << ellipse.b << " angle " << ellipse.angle + << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; - gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; + gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 + << " fs empty border lc rgb 'blue'\n"; } - gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 - << " fs empty border lc rgb 'black'\n"; - gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 - << " fs empty border lc rgb 'pink'\n"; - gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) << " size " << 0.05 - << " fs empty border lc rgb 'green'\n"; + gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " + << 0.05 << " fs empty border lc rgb 'black'\n"; + gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) + << " size " << 0.05 << " fs empty border lc rgb 'pink'\n"; + gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) + << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; - gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " nohead lc rgb 'pink'\n"; - gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n"; + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," + << x_pred.mean()(1) << " nohead lc rgb 'pink'\n"; + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," + << x_final.mean()(1) << " nohead lc rgb 'green'\n"; // old state from 3_1 - gp << "set object " << ++object_counter << " circle center " << 0.5 << "," << 0.5 << " size " << 0.05 << " fs empty border lc rgb 'orange-red'\n"; - gp << "set arrow from " << 0.5 << "," << 0.5 << " to " << -0.00173734 << "," << 0.0766262 << " nohead lc rgb 'orange-red'\n"; + gp << "set object " << ++object_counter << " circle center " << 0.5 << "," << 0.5 << " size " << 0.05 + << " fs empty border lc rgb 'orange-red'\n"; + gp << "set arrow from " << 0.5 << "," << 0.5 << " to " << -0.00173734 << "," << 0.0766262 + << " nohead lc rgb 'orange-red'\n"; vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); - gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle - << " fs empty border lc rgb 'cyan'\n"; + gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," + << gate.b << " angle " << gate.angle << " fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; } TEST(PDAF, predict_next_state_3_3) { - - double gate_threshold = 4; + double gate_threshold = 4; double prob_of_detection = 0.9; double clutter_intensity = 1.0; SimplePDAF pdaf; vortex::prob::Gauss4d x_est(Eigen::Vector4d(-0.55929, 0.0694888, -0.583476, -0.26382), Eigen::Matrix4d::Identity()); - std::vector meas = {{-1.5, 2.5}, {-1.2, 2.7}, {-0.8, 2.3}, {-1.7, 1.9}, {-2.0, 3.0}, {-1.11, 2.04}}; + std::vector meas = { { -1.5, 2.5 }, { -1.2, 2.7 }, { -0.8, 2.3 }, + { -1.7, 1.9 }, { -2.0, 3.0 }, { -1.11, 2.04 } }; auto dyn_model = std::make_shared>(0.5); auto sen_model = std::make_shared>(1.0); - auto [x_final, inside, outside, x_pred, z_pred, x_updated] = - pdaf.predict_next_state(x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); + auto [x_final, inside, outside, x_pred, z_pred, x_updated] = pdaf.predict_next_state( + x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); std::cout << "x_final: " << x_final.mean() << std::endl; Gnuplot gp; @@ -490,56 +527,64 @@ TEST(PDAF, predict_next_state_3_3) int object_counter = 0; - for (const auto &state : x_updated) { + for (const auto& state : x_updated) + { vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2, 2)); vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); - gp << "set object " << ++object_counter << " ellipse center " << ellipse.x << "," << ellipse.y << " size " << ellipse.a << "," << ellipse.b << " angle " - << ellipse.angle << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + gp << "set object " << ++object_counter << " ellipse center " << ellipse.x << "," << ellipse.y << " size " + << ellipse.a << "," << ellipse.b << " angle " << ellipse.angle + << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; - gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; + gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 + << " fs empty border lc rgb 'blue'\n"; } - gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 - << " fs empty border lc rgb 'black'\n"; - gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 - << " fs empty border lc rgb 'pink'\n"; - gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) << " size " << 0.05 - << " fs empty border lc rgb 'green'\n"; + gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " + << 0.05 << " fs empty border lc rgb 'black'\n"; + gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) + << " size " << 0.05 << " fs empty border lc rgb 'pink'\n"; + gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) + << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; - gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " nohead lc rgb 'pink'\n"; - gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n"; + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," + << x_pred.mean()(1) << " nohead lc rgb 'pink'\n"; + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," + << x_final.mean()(1) << " nohead lc rgb 'green'\n"; // old state from 3_1, 3_2 - gp << "set object " << ++object_counter << " circle center " << 0.5 << "," << 0.5 << " size " << 0.05 << " fs empty border lc rgb 'orange-red'\n"; - gp << "set arrow from " << 0.5 << "," << 0.5 << " to " << -0.00173734 << "," << 0.0766262 << " nohead lc rgb 'orange-red'\n"; + gp << "set object " << ++object_counter << " circle center " << 0.5 << "," << 0.5 << " size " << 0.05 + << " fs empty border lc rgb 'orange-red'\n"; + gp << "set arrow from " << 0.5 << "," << 0.5 << " to " << -0.00173734 << "," << 0.0766262 + << " nohead lc rgb 'orange-red'\n"; gp << "set object " << ++object_counter << " circle center " << -0.00173734 << "," << 0.0766262 << " size " << 0.05 << " fs empty border lc rgb 'orange-red'\n"; - gp << "set arrow from " << -0.00173734 << "," << 0.0766262 << " to " << -0.55929 << "," << 0.0694888 << " nohead lc rgb 'orange-red'\n"; + gp << "set arrow from " << -0.00173734 << "," << 0.0766262 << " to " << -0.55929 << "," << 0.0694888 + << " nohead lc rgb 'orange-red'\n"; vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); - gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle - << " fs empty border lc rgb 'cyan'\n"; + gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," + << gate.b << " angle " << gate.angle << " fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; } TEST(PDAF, predict_next_state_3_4) { - - double gate_threshold = 4; + double gate_threshold = 4; double prob_of_detection = 0.9; double clutter_intensity = 1.0; SimplePDAF pdaf; vortex::prob::Gauss4d x_est(Eigen::Vector4d(-1.20613, 0.610616, -0.618037, 0.175242), Eigen::Matrix4d::Identity()); - std::vector meas = {{-2.0, 2.2}, {-1.8, 2.3}, {-2.3, 2.0}, {0.6, 1.5}, {-2.0, 2.0}, {-1.4, 2.5}}; + std::vector meas = { { -2.0, 2.2 }, { -1.8, 2.3 }, { -2.3, 2.0 }, + { 0.6, 1.5 }, { -2.0, 2.0 }, { -1.4, 2.5 } }; auto dyn_model = std::make_shared>(0.5); auto sen_model = std::make_shared>(1.0); - auto [x_final, inside, outside, x_pred, z_pred, x_updated] = - pdaf.predict_next_state(x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); + auto [x_final, inside, outside, x_pred, z_pred, x_updated] = pdaf.predict_next_state( + x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); std::cout << "x_final: " << x_final.mean() << std::endl; Gnuplot gp; @@ -552,38 +597,48 @@ TEST(PDAF, predict_next_state_3_4) int object_counter = 0; - for (const auto &state : x_updated) { + for (const auto& state : x_updated) + { vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2, 2)); vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); - gp << "set object " << ++object_counter << " ellipse center " << ellipse.x << "," << ellipse.y << " size " << ellipse.a << "," << ellipse.b << " angle " - << ellipse.angle << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + gp << "set object " << ++object_counter << " ellipse center " << ellipse.x << "," << ellipse.y << " size " + << ellipse.a << "," << ellipse.b << " angle " << ellipse.angle + << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; - gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; + gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 + << " fs empty border lc rgb 'blue'\n"; } - gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 - << " fs empty border lc rgb 'black'\n"; - gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 - << " fs empty border lc rgb 'pink'\n"; - gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) << " size " << 0.05 - << " fs empty border lc rgb 'green'\n"; + gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " + << 0.05 << " fs empty border lc rgb 'black'\n"; + gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) + << " size " << 0.05 << " fs empty border lc rgb 'pink'\n"; + gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) + << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; - gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " nohead lc rgb 'pink'\n"; - gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n"; + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," + << x_pred.mean()(1) << " nohead lc rgb 'pink'\n"; + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," + << x_final.mean()(1) << " nohead lc rgb 'green'\n"; // old state from 3_1, 3_2, 3_3 - gp << "set object " << ++object_counter << " circle center " << 0.5 << "," << 0.5 << " size " << 0.05 << " fs empty border lc rgb 'orange-red'\n"; - gp << "set arrow from " << 0.5 << "," << 0.5 << " to " << -0.00173734 << "," << 0.0766262 << " nohead lc rgb 'orange-red'\n"; + gp << "set object " << ++object_counter << " circle center " << 0.5 << "," << 0.5 << " size " << 0.05 + << " fs empty border lc rgb 'orange-red'\n"; + gp << "set arrow from " << 0.5 << "," << 0.5 << " to " << -0.00173734 << "," << 0.0766262 + << " nohead lc rgb 'orange-red'\n"; gp << "set object " << ++object_counter << " circle center " << -0.00173734 << "," << 0.0766262 << " size " << 0.05 << " fs empty border lc rgb 'orange-red'\n"; - gp << "set arrow from " << -0.00173734 << "," << 0.0766262 << " to " << -0.55929 << "," << 0.0694888 << " nohead lc rgb 'orange-red'\n"; - gp << "set object " << ++object_counter << " circle center " << -0.55929 << "," << 0.0694888 << " size " << 0.05 << " fs empty border lc rgb 'orange-red'\n"; - gp << "set arrow from " << -0.55929 << "," << 0.0694888 << " to " << -1.20613 << "," << 0.610616 << " nohead lc rgb 'orange-red'\n"; + gp << "set arrow from " << -0.00173734 << "," << 0.0766262 << " to " << -0.55929 << "," << 0.0694888 + << " nohead lc rgb 'orange-red'\n"; + gp << "set object " << ++object_counter << " circle center " << -0.55929 << "," << 0.0694888 << " size " << 0.05 + << " fs empty border lc rgb 'orange-red'\n"; + gp << "set arrow from " << -0.55929 << "," << 0.0694888 << " to " << -1.20613 << "," << 0.610616 + << " nohead lc rgb 'orange-red'\n"; vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); - gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle - << " fs empty border lc rgb 'cyan'\n"; + gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," + << gate.b << " angle " << gate.angle << " fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; } \ No newline at end of file From 9ea647ffd1931f351bfc67b9410760da5d31c1ea Mon Sep 17 00:00:00 2001 From: TristanWolfram Date: Sun, 4 Feb 2024 16:38:23 +0100 Subject: [PATCH 14/31] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Refactor=20PDAF=20cl?= =?UTF-8?q?ass=20and=20apply=20static=20methods?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/vortex_filtering/filters/pdaf.hpp | 76 +++++++++++-------- vortex-filtering/test/pdaf_test.cpp | 71 +++++------------ 2 files changed, 60 insertions(+), 87 deletions(-) diff --git a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp index dc80a43e..6c344df7 100644 --- a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp @@ -7,71 +7,79 @@ #include #include -namespace vortex { -namespace filter { +namespace vortex +{ +namespace filter +{ using std::string; // using std::endl; -template class PDAF { +template +class PDAF +{ public: - using SensModI = typename SensModT::SensModI; - using DynModI = typename DynModT::DynModI; - using DynModPtr = std::shared_ptr; - using SensModPtr = std::shared_ptr; - using EKF = vortex::filter::EKF; - using Gauss_z = typename SensModI::Gauss_z; - using Gauss_x = typename DynModI::Gauss_x; - using Vec_z = typename SensModI::Vec_z; + using SensModI = typename SensModT::SensModI; + using DynModI = typename DynModT::DynModI; + using DynModPtr = std::shared_ptr; + using SensModPtr = std::shared_ptr; + using EKF = vortex::filter::EKF; + using Gauss_z = typename SensModI::Gauss_z; + using Gauss_x = typename DynModI::Gauss_x; + using Vec_z = typename SensModI::Vec_z; using MeasurementsZd = std::vector; - using StatesXd = std::vector; - using GaussMixZd = vortex::prob::GaussianMixture; + using StatesXd = std::vector; + using GaussMixZd = vortex::prob::GaussianMixture; - PDAF() { std::cout << "Created PDAF class!" << std::endl; } + PDAF() = delete; - std::tuple predict_next_state(const Gauss_x &x_est, const MeasurementsZd &z_meas, - double timestep, const DynModPtr &dyn_model, - const SensModPtr &sen_model, double gate_threshold, - double prob_of_detection, double clutter_intensity) const + static std::tuple predict_next_state( + const Gauss_x& x_est, const MeasurementsZd& z_meas, double timestep, const DynModPtr& dyn_model, + const SensModPtr& sen_model, double gate_threshold, double prob_of_detection, double clutter_intensity) { - auto [x_pred, z_pred] = EKF::predict(dyn_model, sen_model, timestep, x_est); + auto [x_pred, z_pred] = EKF::predict(dyn_model, sen_model, timestep, x_est); auto [inside, outside] = apply_gate(z_meas, z_pred, gate_threshold); StatesXd x_updated; - for (const auto &measurement : inside) { + for (const auto& measurement : inside) + { x_updated.push_back(EKF::update(sen_model, x_pred, z_pred, measurement)); } // TODO: return gaussian mixture Gauss_x x_final = get_weighted_average(z_meas, x_updated, z_pred, x_pred, prob_of_detection, clutter_intensity); - return {x_final, inside, outside, x_pred, z_pred, x_updated}; + return { x_final, inside, outside, x_pred, z_pred, x_updated }; } - std::tuple apply_gate(const MeasurementsZd &z_meas, const Gauss_z &z_pred, double gate_threshold) const + static std::tuple apply_gate(const MeasurementsZd& z_meas, const Gauss_z& z_pred, + double gate_threshold) { MeasurementsZd inside_meas; MeasurementsZd outside_meas; - for (const auto &measurement : z_meas) { + for (const auto& measurement : z_meas) + { double distance = z_pred.mahalanobis_distance(measurement); std::cout << "measurement: " << measurement << std::endl; std::cout << "z_pred: " << z_pred.mean() << std::endl; std::cout << "distance: " << distance << std::endl; - if (distance <= gate_threshold) { + if (distance <= gate_threshold) + { inside_meas.push_back(measurement); } - else { + else + { outside_meas.push_back(measurement); } } - return {inside_meas, outside_meas}; + return { inside_meas, outside_meas }; } // Getting weighted average of the predicted states - Gauss_x get_weighted_average(const MeasurementsZd &z_meas, const StatesXd &updated_states, const Gauss_z &z_pred, const Gauss_x &x_pred, - double prob_of_detection, double clutter_intensity) const + static Gauss_x get_weighted_average(const MeasurementsZd& z_meas, const StatesXd& updated_states, const Gauss_z& z_pred, + const Gauss_x& x_pred, double prob_of_detection, double clutter_intensity) { StatesXd states; states.push_back(x_pred); @@ -85,16 +93,18 @@ template class PDAF { } // Getting association probabilities according to textbook p. 123 "Corollary 7.3.3" - Eigen::VectorXd get_weights(const MeasurementsZd &z_meas, const Gauss_z &z_pred, double prob_of_detection, double clutter_intensity) const + static Eigen::VectorXd get_weights(const MeasurementsZd& z_meas, const Gauss_z& z_pred, double prob_of_detection, + double clutter_intensity) { Eigen::VectorXd weights(z_meas.size() + 1); // in case no measurement assosiates with the target double no_association = clutter_intensity * (1 - prob_of_detection); - weights(0) = no_association; + weights(0) = no_association; // measurements associating with the target - for (size_t k = 1; k < z_meas.size() + 1; k++) { + for (size_t k = 1; k < z_meas.size() + 1; k++) + { weights(k) = (prob_of_detection * z_pred.pdf(z_meas.at(k - 1))); } @@ -105,5 +115,5 @@ template class PDAF { } }; -} // namespace filter -} // namespace vortex \ No newline at end of file +} // namespace filter +} // namespace vortex \ No newline at end of file diff --git a/vortex-filtering/test/pdaf_test.cpp b/vortex-filtering/test/pdaf_test.cpp index d0c26f5e..467a802e 100644 --- a/vortex-filtering/test/pdaf_test.cpp +++ b/vortex-filtering/test/pdaf_test.cpp @@ -4,25 +4,18 @@ #include #include -using SimplePDAF = vortex::filter::PDAF, vortex::models::IdentitySensorModel<4, 2>>; - -TEST(PDAF, init) -{ - SimplePDAF pdaf; -} +using PDAF = vortex::filter::PDAF, vortex::models::IdentitySensorModel<4, 2>>; // testing the get_weights function TEST(PDAF, get_weights_is_calculating) { - SimplePDAF pdaf; - double prob_of_detection = 0.8; double clutter_intensity = 1.0; vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); std::vector meas = { { 0.0, 0.0 }, { 2.0, 1.0 } }; - Eigen::VectorXd weights = pdaf.get_weights(meas, z_pred, prob_of_detection, clutter_intensity); + Eigen::VectorXd weights = PDAF::get_weights(meas, z_pred, prob_of_detection, clutter_intensity); std::cout << "weights: " << weights << std::endl; @@ -31,15 +24,13 @@ TEST(PDAF, get_weights_is_calculating) TEST(PDAF, if_no_clutter_first_weight_is_zero) { - SimplePDAF pdaf; - double prob_of_detection = 0.8; double clutter_intensity = 0.0; vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); std::vector meas = { { 0.0, 0.0 }, { 2.0, 1.0 } }; - Eigen::VectorXd weights = pdaf.get_weights(meas, z_pred, prob_of_detection, clutter_intensity); + Eigen::VectorXd weights = PDAF::get_weights(meas, z_pred, prob_of_detection, clutter_intensity); std::cout << "weights: " << weights << std::endl; @@ -48,15 +39,13 @@ TEST(PDAF, if_no_clutter_first_weight_is_zero) TEST(PDAF, weights_are_decreasing_with_distance) { - SimplePDAF pdaf; - double prob_of_detection = 0.8; double clutter_intensity = 1.0; vortex::prob::Gauss2d z_pred(Eigen::Vector2d(1.0, 1.0), Eigen::Matrix2d::Identity()); std::vector meas = { { 2.0, 1.0 }, { 3.0, 1.0 }, { 4.0, 1.0 } }; - Eigen::VectorXd weights = pdaf.get_weights(meas, z_pred, prob_of_detection, clutter_intensity); + Eigen::VectorXd weights = PDAF::get_weights(meas, z_pred, prob_of_detection, clutter_intensity); std::cout << "weights: " << weights << std::endl; @@ -67,8 +56,6 @@ TEST(PDAF, weights_are_decreasing_with_distance) // testing the get_weighted_average function TEST(PDAF, get_weighted_average_is_calculating) { - SimplePDAF pdaf; - double prob_of_detection = 0.8; double clutter_intensity = 1.0; @@ -81,15 +68,13 @@ TEST(PDAF, get_weighted_average_is_calculating) }; vortex::prob::Gauss4d weighted_average = - pdaf.get_weighted_average(meas, updated_states, z_pred, x_pred, prob_of_detection, clutter_intensity); + PDAF::get_weighted_average(meas, updated_states, z_pred, x_pred, prob_of_detection, clutter_intensity); std::cout << "weighted average: " << weighted_average.mean() << std::endl; } TEST(PDAF, average_state_is_in_between_prediction_and_measurement_y_axis) { - SimplePDAF pdaf; - double prob_of_detection = 0.8; double clutter_intensity = 1.0; @@ -100,7 +85,7 @@ TEST(PDAF, average_state_is_in_between_prediction_and_measurement_y_axis) Eigen::Matrix4d::Identity()) }; vortex::prob::Gauss4d weighted_average = - pdaf.get_weighted_average(meas, updated_states, z_pred, x_pred, prob_of_detection, clutter_intensity); + PDAF::get_weighted_average(meas, updated_states, z_pred, x_pred, prob_of_detection, clutter_intensity); EXPECT_GT(weighted_average.mean()(1), x_pred.mean()(1)); EXPECT_GT(weighted_average.mean()(1), z_pred.mean()(1)); @@ -112,8 +97,6 @@ TEST(PDAF, average_state_is_in_between_prediction_and_measurement_y_axis) TEST(PDAF, average_state_is_in_between_prediction_and_measurement_x_axis) { - SimplePDAF pdaf; - double prob_of_detection = 0.8; double clutter_intensity = 1.0; @@ -124,7 +107,7 @@ TEST(PDAF, average_state_is_in_between_prediction_and_measurement_x_axis) Eigen::Matrix4d::Identity()) }; vortex::prob::Gauss4d weighted_average = - pdaf.get_weighted_average(meas, updated_states, z_pred, x_pred, prob_of_detection, clutter_intensity); + PDAF::get_weighted_average(meas, updated_states, z_pred, x_pred, prob_of_detection, clutter_intensity); EXPECT_GT(weighted_average.mean()(0), x_pred.mean()(0)); EXPECT_GT(weighted_average.mean()(0), z_pred.mean()(0)); @@ -136,8 +119,6 @@ TEST(PDAF, average_state_is_in_between_prediction_and_measurement_x_axis) TEST(PDAF, average_state_is_in_between_prediction_and_measurement_both_axes) { - SimplePDAF pdaf; - double prob_of_detection = 0.8; double clutter_intensity = 1.0; @@ -148,7 +129,7 @@ TEST(PDAF, average_state_is_in_between_prediction_and_measurement_both_axes) Eigen::Matrix4d::Identity()) }; vortex::prob::Gauss4d weighted_average = - pdaf.get_weighted_average(meas, updated_states, z_pred, x_pred, prob_of_detection, clutter_intensity); + PDAF::get_weighted_average(meas, updated_states, z_pred, x_pred, prob_of_detection, clutter_intensity); EXPECT_GT(weighted_average.mean()(0), x_pred.mean()(0)); EXPECT_GT(weighted_average.mean()(0), z_pred.mean()(0)); @@ -166,29 +147,25 @@ TEST(PDAF, average_state_is_in_between_prediction_and_measurement_both_axes) // testing the apply_gate function TEST(PDAF, apply_gate_is_calculating) { - SimplePDAF pdaf; - double gate_threshold = 1.8; vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); std::vector meas = { { 0.0, 1.0 }, { 1.0, 0.0 }, { 1.0, 1.0 }, { 0.0, 2.0 }, { 2.0, 0.0 }, { 2.0, 2.0 } }; - auto [inside, outside] = pdaf.apply_gate(meas, z_pred, gate_threshold); + auto [inside, outside] = PDAF::apply_gate(meas, z_pred, gate_threshold); } TEST(PDAF, apply_gate_is_separating_correctly) { double gate_threshold = 3; - SimplePDAF pdaf; - Eigen::Matrix2d cov; cov << 1.0, 0.0, 0.0, 4.0; vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), cov); std::vector meas = { { 0.0, 4.0 }, { 4.0, 0.0 } }; - auto [inside, outside] = pdaf.apply_gate(meas, z_pred, gate_threshold); + auto [inside, outside] = PDAF::apply_gate(meas, z_pred, gate_threshold); EXPECT_EQ(inside.size(), 1u); EXPECT_EQ(outside.size(), 1u); @@ -218,13 +195,11 @@ TEST(PDAF, apply_gate_is_separating_correctly) TEST(PDAF, apply_gate_is_separating_correctly_2) { double gate_threshold = 2.1; - SimplePDAF pdaf; - vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); std::vector meas = { { 0.0, 1.0 }, { 1.0, 0.0 }, { 1.0, 1.0 }, { 0.0, 2.0 }, { 2.0, 0.0 }, { 2.0, 2.0 } }; - auto [inside, outside] = pdaf.apply_gate(meas, z_pred, gate_threshold); + auto [inside, outside] = PDAF::apply_gate(meas, z_pred, gate_threshold); EXPECT_EQ(inside.size(), 5u); EXPECT_EQ(outside.size(), 1u); @@ -255,8 +230,6 @@ TEST(PDAF, predict_next_state_is_calculating) double gate_threshold = 1.12; double prob_of_detection = 0.8; double clutter_intensity = 1.0; - SimplePDAF pdaf; - vortex::prob::Gauss4d x_est(Eigen::Vector4d(0.0, 0.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); std::vector meas = { { 0.0, 1.0 }, { 1.0, 0.0 }, { 1.0, 1.0 }, { 0.0, 2.0 }, { 2.0, 0.0 }, { 2.0, 2.0 } }; @@ -264,7 +237,7 @@ TEST(PDAF, predict_next_state_is_calculating) auto dyn_model = std::make_shared>(1.0); auto sen_model = std::make_shared>(1.0); - auto [x_final, inside, outside, x_pred, z_pred, x_updated] = pdaf.predict_next_state( + auto [x_final, inside, outside, x_pred, z_pred, x_updated] = PDAF::predict_next_state( x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); std::cout << "x_final: " << x_final.mean() << std::endl; @@ -315,8 +288,6 @@ TEST(PDAF, predict_next_state_2) double gate_threshold = 2; double prob_of_detection = 0.8; double clutter_intensity = 1.0; - SimplePDAF pdaf; - vortex::prob::Gauss4d x_est(Eigen::Vector4d(1.0, 1.5, -2.0, 0.0), Eigen::Matrix4d::Identity()); std::vector meas = { { -3.0, -3.0 }, { 0.0, 0.0 }, { -1.2, 1.5 }, { -2.0, -2.0 }, { 2.0, 0.0 }, { -1.0, 1.0 } }; @@ -324,7 +295,7 @@ TEST(PDAF, predict_next_state_2) auto dyn_model = std::make_shared>(1.0); auto sen_model = std::make_shared>(0.5); - auto [x_final, inside, outside, x_pred, z_pred, x_updated] = pdaf.predict_next_state( + auto [x_final, inside, outside, x_pred, z_pred, x_updated] = PDAF::predict_next_state( x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); std::cout << "x_final: " << x_final.mean() << std::endl; @@ -375,8 +346,6 @@ TEST(PDAF, predict_next_state_3_1) double gate_threshold = 4; double prob_of_detection = 0.9; double clutter_intensity = 1.0; - SimplePDAF pdaf; - vortex::prob::Gauss4d x_est(Eigen::Vector4d(0.5, 0.5, -0.75, -0.75), Eigen::Matrix4d::Identity()); std::vector meas = { { 0.0, 0.5 }, { 0.2, 0.2 }, { 0.8, 2.3 }, { 0.5, 0.0 }, { 4.2, 2.7 }, { 1.4, 2.5 } }; @@ -384,7 +353,7 @@ TEST(PDAF, predict_next_state_3_1) auto dyn_model = std::make_shared>(0.5); auto sen_model = std::make_shared>(1.0); - auto [x_final, inside, outside, x_pred, z_pred, x_updated] = pdaf.predict_next_state( + auto [x_final, inside, outside, x_pred, z_pred, x_updated] = PDAF::predict_next_state( x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); std::cout << "x_final: " << x_final.mean() << std::endl; @@ -437,8 +406,6 @@ TEST(PDAF, predict_next_state_3_2) double gate_threshold = 4; double prob_of_detection = 0.9; double clutter_intensity = 1.0; - SimplePDAF pdaf; - vortex::prob::Gauss4d x_est(Eigen::Vector4d(-0.00173734, 0.0766262, -0.614584, -0.57184), Eigen::Matrix4d::Identity()); std::vector meas = { { -0.5, 2.0 }, { -0.23, 0.5 }, { -2.0, 3.4 }, @@ -447,7 +414,7 @@ TEST(PDAF, predict_next_state_3_2) auto dyn_model = std::make_shared>(0.5); auto sen_model = std::make_shared>(1.0); - auto [x_final, inside, outside, x_pred, z_pred, x_updated] = pdaf.predict_next_state( + auto [x_final, inside, outside, x_pred, z_pred, x_updated] = PDAF::predict_next_state( x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); std::cout << "x_final: " << x_final.mean() << std::endl; @@ -504,8 +471,6 @@ TEST(PDAF, predict_next_state_3_3) double gate_threshold = 4; double prob_of_detection = 0.9; double clutter_intensity = 1.0; - SimplePDAF pdaf; - vortex::prob::Gauss4d x_est(Eigen::Vector4d(-0.55929, 0.0694888, -0.583476, -0.26382), Eigen::Matrix4d::Identity()); std::vector meas = { { -1.5, 2.5 }, { -1.2, 2.7 }, { -0.8, 2.3 }, { -1.7, 1.9 }, { -2.0, 3.0 }, { -1.11, 2.04 } }; @@ -513,7 +478,7 @@ TEST(PDAF, predict_next_state_3_3) auto dyn_model = std::make_shared>(0.5); auto sen_model = std::make_shared>(1.0); - auto [x_final, inside, outside, x_pred, z_pred, x_updated] = pdaf.predict_next_state( + auto [x_final, inside, outside, x_pred, z_pred, x_updated] = PDAF::predict_next_state( x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); std::cout << "x_final: " << x_final.mean() << std::endl; @@ -574,8 +539,6 @@ TEST(PDAF, predict_next_state_3_4) double gate_threshold = 4; double prob_of_detection = 0.9; double clutter_intensity = 1.0; - SimplePDAF pdaf; - vortex::prob::Gauss4d x_est(Eigen::Vector4d(-1.20613, 0.610616, -0.618037, 0.175242), Eigen::Matrix4d::Identity()); std::vector meas = { { -2.0, 2.2 }, { -1.8, 2.3 }, { -2.3, 2.0 }, { 0.6, 1.5 }, { -2.0, 2.0 }, { -1.4, 2.5 } }; @@ -583,7 +546,7 @@ TEST(PDAF, predict_next_state_3_4) auto dyn_model = std::make_shared>(0.5); auto sen_model = std::make_shared>(1.0); - auto [x_final, inside, outside, x_pred, z_pred, x_updated] = pdaf.predict_next_state( + auto [x_final, inside, outside, x_pred, z_pred, x_updated] = PDAF::predict_next_state( x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); std::cout << "x_final: " << x_final.mean() << std::endl; From 7d4f712b9f4286662aa33bf34728f3e500ca4580 Mon Sep 17 00:00:00 2001 From: TristanWolfram Date: Sun, 11 Feb 2024 12:49:32 +0100 Subject: [PATCH 15/31] fixed gate bug --- .../include/vortex_filtering/filters/pdaf.hpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp index 6c344df7..146fd4c5 100644 --- a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp @@ -47,12 +47,12 @@ class PDAF } // TODO: return gaussian mixture - Gauss_x x_final = get_weighted_average(z_meas, x_updated, z_pred, x_pred, prob_of_detection, clutter_intensity); + Gauss_x x_final = get_weighted_average(inside, x_updated, z_pred, x_pred, prob_of_detection, clutter_intensity); return { x_final, inside, outside, x_pred, z_pred, x_updated }; } static std::tuple apply_gate(const MeasurementsZd& z_meas, const Gauss_z& z_pred, - double gate_threshold) + double gate_threshold) { MeasurementsZd inside_meas; MeasurementsZd outside_meas; @@ -78,8 +78,9 @@ class PDAF } // Getting weighted average of the predicted states - static Gauss_x get_weighted_average(const MeasurementsZd& z_meas, const StatesXd& updated_states, const Gauss_z& z_pred, - const Gauss_x& x_pred, double prob_of_detection, double clutter_intensity) + static Gauss_x get_weighted_average(const MeasurementsZd& z_meas, const StatesXd& updated_states, + const Gauss_z& z_pred, const Gauss_x& x_pred, double prob_of_detection, + double clutter_intensity) { StatesXd states; states.push_back(x_pred); @@ -94,7 +95,7 @@ class PDAF // Getting association probabilities according to textbook p. 123 "Corollary 7.3.3" static Eigen::VectorXd get_weights(const MeasurementsZd& z_meas, const Gauss_z& z_pred, double prob_of_detection, - double clutter_intensity) + double clutter_intensity) { Eigen::VectorXd weights(z_meas.size() + 1); From 9da7b3f6c3bd2da294c68d4513e6f865fbdbb0df Mon Sep 17 00:00:00 2001 From: etfroeland Date: Sun, 11 Feb 2024 14:04:19 +0100 Subject: [PATCH 16/31] fixed ipda :) --- .../include/vortex_filtering/filters/ipda.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp index a5dfd8e5..c43a349f 100644 --- a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp @@ -36,7 +36,7 @@ class IPDA { double summed = 0; for(const Vec_z &measurement : measurements){ - summed += z_pred.pfd(measurement); + summed += z_pred.pdf(measurement); } double l_k = 1 - probability_of_detection + probability_of_detection / clutter_intensity * summed; @@ -53,7 +53,7 @@ class IPDA { double prob_of_survival, double survive_est, double clutter_intensity - ) const + ) { auto [x_pred, z_pred] = EKF::predict(dyn_model, sen_model, timestep, x_est); auto [inside, outside] = apply_gate(z_meas, z_pred, gate_threshold); @@ -64,14 +64,14 @@ class IPDA { } Gauss_x x_final = get_weighted_average( - z_meas, + inside, x_updated, z_pred, x_pred, prob_of_detection, clutter_intensity); - double existence_probability = get_existence_probability(z_meas, prob_of_survival, survive_est, prob_of_detection, clutter_intensity, z_pred); + double existence_probability = get_existence_probability(inside, prob_of_survival, survive_est, prob_of_detection, clutter_intensity, z_pred); return {x_final, existence_probability, inside, outside, x_pred, z_pred, x_updated}; } static std::tuple, std::vector> apply_gate( From a0571b03e7e6209a15b6b2145cd5098f357cd5ff Mon Sep 17 00:00:00 2001 From: TristanWolfram Date: Sun, 11 Feb 2024 14:02:43 +0100 Subject: [PATCH 17/31] removed prints --- vortex-filtering/include/vortex_filtering/filters/pdaf.hpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp index 146fd4c5..97d0e61f 100644 --- a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp @@ -60,9 +60,6 @@ class PDAF for (const auto& measurement : z_meas) { double distance = z_pred.mahalanobis_distance(measurement); - std::cout << "measurement: " << measurement << std::endl; - std::cout << "z_pred: " << z_pred.mean() << std::endl; - std::cout << "distance: " << distance << std::endl; if (distance <= gate_threshold) { From 5f240eac59eca878ef891efc6ba5542c6d43da9e Mon Sep 17 00:00:00 2001 From: TristanWolfram Date: Sun, 11 Feb 2024 14:07:27 +0100 Subject: [PATCH 18/31] Refactor IPDA class and add IPDA unit tests --- .../include/vortex_filtering/filters/ipda.hpp | 165 ++++++++---------- vortex-filtering/test/ipda_test.cpp | 15 ++ 2 files changed, 88 insertions(+), 92 deletions(-) create mode 100644 vortex-filtering/test/ipda_test.cpp diff --git a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp index c43a349f..522227cf 100644 --- a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp @@ -8,105 +8,86 @@ #include #include -namespace vortex::filter { -template -class IPDA { +namespace vortex::filter +{ +template +class IPDA +{ public: - using SensModI = typename SensModT::SensModI; - using DynModI = typename DynModT::DynModI; - using DynModPtr = std::shared_ptr; - using SensModPtr = std::shared_ptr; - using EKF = vortex::filter::EKF; - using Gauss_z = typename SensModI::Gauss_z; - using Gauss_x = typename DynModI::Gauss_x; - using Vec_z = typename SensModI::Vec_z; - using GaussMix = vortex::prob::GaussianMixture; - using FAKE_PDAF = vortex::filter::PDAF; - IPDA(){ - + using SensModI = typename SensModT::SensModI; + using DynModI = typename DynModT::DynModI; + using DynModPtr = std::shared_ptr; + using SensModPtr = std::shared_ptr; + using EKF = vortex::filter::EKF; + using Gauss_z = typename SensModI::Gauss_z; + using Gauss_x = typename DynModI::Gauss_x; + using Vec_z = typename SensModI::Vec_z; + using GaussMix = vortex::prob::GaussianMixture; + using FAKE_PDAF = vortex::filter::PDAF; + IPDA() + { + } + /// @brief + /// @param measurements Measurements to iterate over + /// @param probability_of_survival How likely the object is to survive (Ps) + /// @param probability_of_detection How likely the object is to be detected (Pd) + /// @param standard_deviation Standard deviation of the measurements (Sk) + /// @param lambda Lambda value for the Poisson distribution (Lambda) + static double get_existence_probability(const std::vector& measurements, double probability_of_survival, + double last_detection_probability_, double probability_of_detection, + double clutter_intensity, Gauss_z& z_pred) + { + double predicted_existence_probability = probability_of_survival * last_detection_probability_; // Finds Ps + + double summed = 0; + for (const Vec_z& measurement : measurements) + { + summed += z_pred.pdf(measurement); } - /// @brief - /// @param measurements Measurements to iterate over - /// @param probability_of_survival How likely the object is to survive (Ps) - /// @param probability_of_detection How likely the object is to be detected (Pd) - /// @param standard_deviation Standard deviation of the measurements (Sk) - /// @param lambda Lambda value for the Poisson distribution (Lambda) - static double get_existence_probability(const std::vector &measurements, double probability_of_survival, double last_detection_probability_, double probability_of_detection, double clutter_intensity, Gauss_z &z_pred){ - double predicted_existence_probability = probability_of_survival * last_detection_probability_; // Finds Ps + double l_k = 1 - probability_of_detection + probability_of_detection / clutter_intensity * summed; - double summed = 0; - for(const Vec_z &measurement : measurements){ - summed += z_pred.pdf(measurement); - } - double l_k = 1 - probability_of_detection + probability_of_detection / clutter_intensity * summed; + return (l_k * predicted_existence_probability) / (1 - (1 - l_k) * predicted_existence_probability); + } + static std::tuple, std::vector, Gauss_x, Gauss_z, std::vector> + step(const Gauss_x& x_est, const std::vector& z_meas, double timestep, const DynModPtr& dyn_model, + const SensModPtr& sen_model, double gate_threshold, double prob_of_detection, double prob_of_survival, + double survive_est, double clutter_intensity) + { + auto [x_pred, z_pred] = EKF::predict(dyn_model, sen_model, timestep, x_est); + auto [inside, outside] = apply_gate(z_meas, z_pred, gate_threshold); - return (l_k * predicted_existence_probability) / (1 - (1 - l_k) * predicted_existence_probability); - } - static std::tuple, std::vector, Gauss_x, Gauss_z, std::vector> step( - const Gauss_x& x_est, - const std::vector& z_meas, - double timestep, - const DynModPtr& dyn_model, - const SensModPtr& sen_model, - double gate_threshold, - double prob_of_detection, - double prob_of_survival, - double survive_est, - double clutter_intensity - ) + std::vector x_updated; + for (const auto& measurement : inside) { - auto [x_pred, z_pred] = EKF::predict(dyn_model, sen_model, timestep, x_est); - auto [inside, outside] = apply_gate(z_meas, z_pred, gate_threshold); - - std::vector x_updated; - for (const auto& measurement : inside) { - x_updated.push_back(EKF::update(sen_model, x_pred, z_pred, measurement)); - } + x_updated.push_back(EKF::update(sen_model, x_pred, z_pred, measurement)); + } - Gauss_x x_final = get_weighted_average( - inside, - x_updated, - z_pred, - x_pred, - prob_of_detection, - clutter_intensity); + Gauss_x x_final = get_weighted_average(inside, x_updated, z_pred, x_pred, prob_of_detection, clutter_intensity); - double existence_probability = get_existence_probability(inside, prob_of_survival, survive_est, prob_of_detection, clutter_intensity, z_pred); - return {x_final, existence_probability, inside, outside, x_pred, z_pred, x_updated}; - } - static std::tuple, std::vector> apply_gate( - const std::vector& z_meas, - const Gauss_z& z_pred, - double gate_threshold - ) { - FAKE_PDAF pdaf; - return pdaf.apply_gate(z_meas, z_pred, gate_threshold); - } - static Gauss_x get_weighted_average( - const std::vector& z_meas, - const std::vector& updated_states, - const Gauss_z& z_pred, - const Gauss_x& x_pred, - double prob_of_detection, - double clutter_intensity - ){ - FAKE_PDAF pdaf; - return pdaf.get_weighted_average(z_meas, updated_states, z_pred, x_pred, prob_of_detection, clutter_intensity); - } + double existence_probability = + get_existence_probability(inside, prob_of_survival, survive_est, prob_of_detection, clutter_intensity, z_pred); + return { x_final, existence_probability, inside, outside, x_pred, z_pred, x_updated }; + } + static std::tuple, std::vector> apply_gate(const std::vector& z_meas, + const Gauss_z& z_pred, double gate_threshold) + { + FAKE_PDAF pdaf; + return pdaf.apply_gate(z_meas, z_pred, gate_threshold); + } + static Gauss_x get_weighted_average(const std::vector& z_meas, const std::vector& updated_states, + const Gauss_z& z_pred, const Gauss_x& x_pred, double prob_of_detection, + double clutter_intensity) + { + FAKE_PDAF pdaf; + return pdaf.get_weighted_average(z_meas, updated_states, z_pred, x_pred, prob_of_detection, clutter_intensity); + } - Eigen::VectorXd get_weights( - const std::vector& z_meas, - const Gauss_z& z_pred, - double prob_of_detection, - double clutter_intensity - ){ - FAKE_PDAF pdaf; - return pdaf.get_weights(z_meas, z_pred, prob_of_detection, clutter_intensity); - } + Eigen::VectorXd get_weights(const std::vector& z_meas, const Gauss_z& z_pred, double prob_of_detection, + double clutter_intensity) + { + FAKE_PDAF pdaf; + return pdaf.get_weights(z_meas, z_pred, prob_of_detection, clutter_intensity); + } }; } // namespace vortex::filter -#endif // IPDA_HPP - - - - +#endif // IPDA_HPP diff --git a/vortex-filtering/test/ipda_test.cpp b/vortex-filtering/test/ipda_test.cpp new file mode 100644 index 00000000..50133456 --- /dev/null +++ b/vortex-filtering/test/ipda_test.cpp @@ -0,0 +1,15 @@ +#include +#include +#include +#include +#include + +using IPDA = vortex::filter::IPDA, vortex::models::IdentitySensorModel<4, 2>>; + +TEST(IPDA, ipda_runs) +{ +} + +TEST(IPDA, get_existence_probability_is_calculating) +{ +} \ No newline at end of file From c09c6f0487b6fb0452ee1bf389dbf70d9a410f46 Mon Sep 17 00:00:00 2001 From: TristanWolfram Date: Sun, 11 Feb 2024 14:16:00 +0100 Subject: [PATCH 19/31] removed fake pdaf in ipda --- .../include/vortex_filtering/filters/ipda.hpp | 34 +++---------------- 1 file changed, 5 insertions(+), 29 deletions(-) diff --git a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp index 522227cf..9ce5f3ef 100644 --- a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp @@ -1,6 +1,3 @@ -#ifndef IPDA_HPP -#define IPDA_HPP - #pragma once #include #include @@ -24,9 +21,7 @@ class IPDA using Vec_z = typename SensModI::Vec_z; using GaussMix = vortex::prob::GaussianMixture; using FAKE_PDAF = vortex::filter::PDAF; - IPDA() - { - } + IPDA() = delete; /// @brief /// @param measurements Measurements to iterate over /// @param probability_of_survival How likely the object is to survive (Ps) @@ -48,13 +43,14 @@ class IPDA return (l_k * predicted_existence_probability) / (1 - (1 - l_k) * predicted_existence_probability); } + static std::tuple, std::vector, Gauss_x, Gauss_z, std::vector> step(const Gauss_x& x_est, const std::vector& z_meas, double timestep, const DynModPtr& dyn_model, const SensModPtr& sen_model, double gate_threshold, double prob_of_detection, double prob_of_survival, double survive_est, double clutter_intensity) { auto [x_pred, z_pred] = EKF::predict(dyn_model, sen_model, timestep, x_est); - auto [inside, outside] = apply_gate(z_meas, z_pred, gate_threshold); + auto [inside, outside] = vortex::filter::PDAF::apply_gate(z_meas, z_pred, gate_threshold); std::vector x_updated; for (const auto& measurement : inside) @@ -62,32 +58,12 @@ class IPDA x_updated.push_back(EKF::update(sen_model, x_pred, z_pred, measurement)); } - Gauss_x x_final = get_weighted_average(inside, x_updated, z_pred, x_pred, prob_of_detection, clutter_intensity); + Gauss_x x_final = vortex::filter::PDAF::get_weighted_average(inside, x_updated, z_pred, x_pred, prob_of_detection, + clutter_intensity); double existence_probability = get_existence_probability(inside, prob_of_survival, survive_est, prob_of_detection, clutter_intensity, z_pred); return { x_final, existence_probability, inside, outside, x_pred, z_pred, x_updated }; } - static std::tuple, std::vector> apply_gate(const std::vector& z_meas, - const Gauss_z& z_pred, double gate_threshold) - { - FAKE_PDAF pdaf; - return pdaf.apply_gate(z_meas, z_pred, gate_threshold); - } - static Gauss_x get_weighted_average(const std::vector& z_meas, const std::vector& updated_states, - const Gauss_z& z_pred, const Gauss_x& x_pred, double prob_of_detection, - double clutter_intensity) - { - FAKE_PDAF pdaf; - return pdaf.get_weighted_average(z_meas, updated_states, z_pred, x_pred, prob_of_detection, clutter_intensity); - } - - Eigen::VectorXd get_weights(const std::vector& z_meas, const Gauss_z& z_pred, double prob_of_detection, - double clutter_intensity) - { - FAKE_PDAF pdaf; - return pdaf.get_weights(z_meas, z_pred, prob_of_detection, clutter_intensity); - } }; } // namespace vortex::filter -#endif // IPDA_HPP From 4800f266bc2822cbbe4be36dd6adee16ba40bec0 Mon Sep 17 00:00:00 2001 From: TristanWolfram Date: Sun, 11 Feb 2024 15:32:44 +0100 Subject: [PATCH 20/31] Add IPDA test and update IPDA class*** --- .../include/vortex_filtering/filters/ipda.hpp | 13 ++++---- vortex-filtering/test/ipda_test.cpp | 33 +++++++++++++++++++ 2 files changed, 40 insertions(+), 6 deletions(-) diff --git a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp index 9ce5f3ef..e856f8b2 100644 --- a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp @@ -20,14 +20,15 @@ class IPDA using Gauss_x = typename DynModI::Gauss_x; using Vec_z = typename SensModI::Vec_z; using GaussMix = vortex::prob::GaussianMixture; - using FAKE_PDAF = vortex::filter::PDAF; + using PDAF = vortex::filter::PDAF, vortex::models::IdentitySensorModel<4, 2>>; IPDA() = delete; /// @brief /// @param measurements Measurements to iterate over /// @param probability_of_survival How likely the object is to survive (Ps) /// @param probability_of_detection How likely the object is to be detected (Pd) - /// @param standard_deviation Standard deviation of the measurements (Sk) - /// @param lambda Lambda value for the Poisson distribution (Lambda) + /// @param clutter_intensity How likely it is to have a false positive + /// @param z_pred The predicted measurement + /// @return The existence probability static double get_existence_probability(const std::vector& measurements, double probability_of_survival, double last_detection_probability_, double probability_of_detection, double clutter_intensity, Gauss_z& z_pred) @@ -50,7 +51,7 @@ class IPDA double survive_est, double clutter_intensity) { auto [x_pred, z_pred] = EKF::predict(dyn_model, sen_model, timestep, x_est); - auto [inside, outside] = vortex::filter::PDAF::apply_gate(z_meas, z_pred, gate_threshold); + auto [inside, outside] = PDAF::apply_gate(z_meas, z_pred, gate_threshold); std::vector x_updated; for (const auto& measurement : inside) @@ -58,8 +59,8 @@ class IPDA x_updated.push_back(EKF::update(sen_model, x_pred, z_pred, measurement)); } - Gauss_x x_final = vortex::filter::PDAF::get_weighted_average(inside, x_updated, z_pred, x_pred, prob_of_detection, - clutter_intensity); + Gauss_x x_final = + PDAF::get_weighted_average(inside, x_updated, z_pred, x_pred, prob_of_detection, clutter_intensity); double existence_probability = get_existence_probability(inside, prob_of_survival, survive_est, prob_of_detection, clutter_intensity, z_pred); diff --git a/vortex-filtering/test/ipda_test.cpp b/vortex-filtering/test/ipda_test.cpp index 50133456..c6a35a8d 100644 --- a/vortex-filtering/test/ipda_test.cpp +++ b/vortex-filtering/test/ipda_test.cpp @@ -8,8 +8,41 @@ using IPDA = vortex::filter::IPDA, vortex::m TEST(IPDA, ipda_runs) { + double gate_threshold = 1.12; + double prob_of_detection = 0.8; + double clutter_intensity = 1.0; + double probability_of_survival = 0.9; + double last_detection_probability = 0.85; + + vortex::prob::Gauss4d x_est(Eigen::Vector4d(0.0, 0.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); + std::vector meas = { { 0.0, 1.0 }, { 1.0, 0.0 }, { 1.0, 1.0 }, + { 0.0, 2.0 }, { 2.0, 0.0 }, { 2.0, 2.0 } }; + + auto dyn_model = std::make_shared>(1.0); + auto sen_model = std::make_shared>(1.0); + + auto [x_final, existence_pred, inside, outside, x_pred, z_pred, x_updated] = + IPDA::step(x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, probability_of_survival, + last_detection_probability, clutter_intensity); + + std::cout << "Existence probability: " << existence_pred << std::endl; } TEST(IPDA, get_existence_probability_is_calculating) { + double prob_of_detection = 0.8; + double clutter_intensity = 1.0; + double probability_of_survival = 0.9; + double last_detection_probability = 0.9; + + std::vector meas = { { 0.0, 1.0 }, { 1.0, 0.0 }, { 1.0, 1.0 }, + { 0.0, 2.0 }, { 2.0, 0.0 }, { 2.0, 2.0 } }; + + vortex::prob::Gauss2d z_pred; + z_pred = vortex::prob::Gauss2d(Eigen::Vector2d(1.0, 1.0), Eigen::Matrix2d::Identity() * 0.1); + + double existence_probability = IPDA::get_existence_probability( + meas, probability_of_survival, last_detection_probability, prob_of_detection, clutter_intensity, z_pred); + + std::cout << "Existence probability: " << existence_probability << std::endl; } \ No newline at end of file From d3dcbd108dbe6cf6fd13939345517da51f2658dc Mon Sep 17 00:00:00 2001 From: TristanWolfram Date: Sun, 11 Feb 2024 17:12:12 +0100 Subject: [PATCH 21/31] Refactor PDAF::predict_next_state to PDAF::step --- .../include/vortex_filtering/filters/ipda.hpp | 13 ++-------- .../include/vortex_filtering/filters/pdaf.hpp | 6 ++--- vortex-filtering/test/pdaf_test.cpp | 24 +++++++++---------- 3 files changed, 17 insertions(+), 26 deletions(-) diff --git a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp index e856f8b2..e91a507e 100644 --- a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp @@ -50,17 +50,8 @@ class IPDA const SensModPtr& sen_model, double gate_threshold, double prob_of_detection, double prob_of_survival, double survive_est, double clutter_intensity) { - auto [x_pred, z_pred] = EKF::predict(dyn_model, sen_model, timestep, x_est); - auto [inside, outside] = PDAF::apply_gate(z_meas, z_pred, gate_threshold); - - std::vector x_updated; - for (const auto& measurement : inside) - { - x_updated.push_back(EKF::update(sen_model, x_pred, z_pred, measurement)); - } - - Gauss_x x_final = - PDAF::get_weighted_average(inside, x_updated, z_pred, x_pred, prob_of_detection, clutter_intensity); + auto [x_final, inside, outside, x_pred, z_pred, x_updated] = + PDAF::step(x_est, z_meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); double existence_probability = get_existence_probability(inside, prob_of_survival, survive_est, prob_of_detection, clutter_intensity, z_pred); diff --git a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp index 97d0e61f..a545cff7 100644 --- a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp @@ -33,9 +33,9 @@ class PDAF PDAF() = delete; - static std::tuple predict_next_state( - const Gauss_x& x_est, const MeasurementsZd& z_meas, double timestep, const DynModPtr& dyn_model, - const SensModPtr& sen_model, double gate_threshold, double prob_of_detection, double clutter_intensity) + static std::tuple + step(const Gauss_x& x_est, const MeasurementsZd& z_meas, double timestep, const DynModPtr& dyn_model, + const SensModPtr& sen_model, double gate_threshold, double prob_of_detection, double clutter_intensity) { auto [x_pred, z_pred] = EKF::predict(dyn_model, sen_model, timestep, x_est); auto [inside, outside] = apply_gate(z_meas, z_pred, gate_threshold); diff --git a/vortex-filtering/test/pdaf_test.cpp b/vortex-filtering/test/pdaf_test.cpp index 467a802e..e9d24882 100644 --- a/vortex-filtering/test/pdaf_test.cpp +++ b/vortex-filtering/test/pdaf_test.cpp @@ -237,8 +237,8 @@ TEST(PDAF, predict_next_state_is_calculating) auto dyn_model = std::make_shared>(1.0); auto sen_model = std::make_shared>(1.0); - auto [x_final, inside, outside, x_pred, z_pred, x_updated] = PDAF::predict_next_state( - x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); + auto [x_final, inside, outside, x_pred, z_pred, x_updated] = + PDAF::step(x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); std::cout << "x_final: " << x_final.mean() << std::endl; Gnuplot gp; @@ -295,8 +295,8 @@ TEST(PDAF, predict_next_state_2) auto dyn_model = std::make_shared>(1.0); auto sen_model = std::make_shared>(0.5); - auto [x_final, inside, outside, x_pred, z_pred, x_updated] = PDAF::predict_next_state( - x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); + auto [x_final, inside, outside, x_pred, z_pred, x_updated] = + PDAF::step(x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); std::cout << "x_final: " << x_final.mean() << std::endl; Gnuplot gp; @@ -353,8 +353,8 @@ TEST(PDAF, predict_next_state_3_1) auto dyn_model = std::make_shared>(0.5); auto sen_model = std::make_shared>(1.0); - auto [x_final, inside, outside, x_pred, z_pred, x_updated] = PDAF::predict_next_state( - x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); + auto [x_final, inside, outside, x_pred, z_pred, x_updated] = + PDAF::step(x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); std::cout << "x_final: " << x_final.mean() << std::endl; Gnuplot gp; @@ -414,8 +414,8 @@ TEST(PDAF, predict_next_state_3_2) auto dyn_model = std::make_shared>(0.5); auto sen_model = std::make_shared>(1.0); - auto [x_final, inside, outside, x_pred, z_pred, x_updated] = PDAF::predict_next_state( - x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); + auto [x_final, inside, outside, x_pred, z_pred, x_updated] = + PDAF::step(x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); std::cout << "x_final: " << x_final.mean() << std::endl; Gnuplot gp; @@ -478,8 +478,8 @@ TEST(PDAF, predict_next_state_3_3) auto dyn_model = std::make_shared>(0.5); auto sen_model = std::make_shared>(1.0); - auto [x_final, inside, outside, x_pred, z_pred, x_updated] = PDAF::predict_next_state( - x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); + auto [x_final, inside, outside, x_pred, z_pred, x_updated] = + PDAF::step(x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); std::cout << "x_final: " << x_final.mean() << std::endl; Gnuplot gp; @@ -546,8 +546,8 @@ TEST(PDAF, predict_next_state_3_4) auto dyn_model = std::make_shared>(0.5); auto sen_model = std::make_shared>(1.0); - auto [x_final, inside, outside, x_pred, z_pred, x_updated] = PDAF::predict_next_state( - x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); + auto [x_final, inside, outside, x_pred, z_pred, x_updated] = + PDAF::step(x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); std::cout << "x_final: " << x_final.mean() << std::endl; Gnuplot gp; From aeeb0f42da3303bc6ef1ff981dde474f08961fd7 Mon Sep 17 00:00:00 2001 From: TristanWolfram Date: Tue, 20 Feb 2024 19:11:53 +0100 Subject: [PATCH 22/31] =?UTF-8?q?=F0=9F=92=AB=20adden=20min=20and=20max=20?= =?UTF-8?q?distance=20threshold?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/vortex_filtering/filters/ipda.hpp | 17 ++-- .../include/vortex_filtering/filters/pdaf.hpp | 31 +++++-- vortex-filtering/test/ipda_test.cpp | 16 ++-- vortex-filtering/test/pdaf_test.cpp | 82 ++++++++++--------- 4 files changed, 87 insertions(+), 59 deletions(-) diff --git a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp index e91a507e..d1368708 100644 --- a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp @@ -22,6 +22,12 @@ class IPDA using GaussMix = vortex::prob::GaussianMixture; using PDAF = vortex::filter::PDAF, vortex::models::IdentitySensorModel<4, 2>>; IPDA() = delete; + + struct Config : public PDAF::Config + { + double prob_of_survival = 1.0; + }; + /// @brief /// @param measurements Measurements to iterate over /// @param probability_of_survival How likely the object is to survive (Ps) @@ -46,15 +52,14 @@ class IPDA } static std::tuple, std::vector, Gauss_x, Gauss_z, std::vector> - step(const Gauss_x& x_est, const std::vector& z_meas, double timestep, const DynModPtr& dyn_model, - const SensModPtr& sen_model, double gate_threshold, double prob_of_detection, double prob_of_survival, - double survive_est, double clutter_intensity) + step(const DynModPtr& dyn_model, const SensModPtr& sen_model, double timestep, const Gauss_x& x_est, + const std::vector& z_meas, double survive_est, const IPDA::Config& config) { auto [x_final, inside, outside, x_pred, z_pred, x_updated] = - PDAF::step(x_est, z_meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); + PDAF::step(dyn_model, sen_model, timestep, x_est, z_meas, static_cast(config)); - double existence_probability = - get_existence_probability(inside, prob_of_survival, survive_est, prob_of_detection, clutter_intensity, z_pred); + double existence_probability = get_existence_probability( + inside, config.prob_of_survival, survive_est, config.prob_of_detection, config.clutter_intensity, z_pred); return { x_final, existence_probability, inside, outside, x_pred, z_pred, x_updated }; } }; diff --git a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp index a545cff7..3a2a6fbb 100644 --- a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp @@ -31,14 +31,24 @@ class PDAF using StatesXd = std::vector; using GaussMixZd = vortex::prob::GaussianMixture; + struct Config + { + double mahalanobis_threshold = 1.0; + double min_gate_threshold = 0.0; + double max_gate_threshold = HUGE_VAL; + double prob_of_detection = 1.0; + double clutter_intensity = 1.0; + }; + PDAF() = delete; static std::tuple - step(const Gauss_x& x_est, const MeasurementsZd& z_meas, double timestep, const DynModPtr& dyn_model, - const SensModPtr& sen_model, double gate_threshold, double prob_of_detection, double clutter_intensity) + step(const DynModPtr& dyn_model, const SensModPtr& sen_model, double timestep, const Gauss_x& x_est, + const MeasurementsZd& z_meas, const Config& config) { auto [x_pred, z_pred] = EKF::predict(dyn_model, sen_model, timestep, x_est); - auto [inside, outside] = apply_gate(z_meas, z_pred, gate_threshold); + auto [inside, outside] = + apply_gate(z_meas, z_pred, config.mahalanobis_threshold, config.min_gate_threshold, config.max_gate_threshold); StatesXd x_updated; for (const auto& measurement : inside) @@ -46,22 +56,25 @@ class PDAF x_updated.push_back(EKF::update(sen_model, x_pred, z_pred, measurement)); } - // TODO: return gaussian mixture - Gauss_x x_final = get_weighted_average(inside, x_updated, z_pred, x_pred, prob_of_detection, clutter_intensity); + Gauss_x x_final = + get_weighted_average(inside, x_updated, z_pred, x_pred, config.prob_of_detection, config.clutter_intensity); return { x_final, inside, outside, x_pred, z_pred, x_updated }; } static std::tuple apply_gate(const MeasurementsZd& z_meas, const Gauss_z& z_pred, - double gate_threshold) + double mahalanobis_threshold, + double min_gate_threshold = 0.0, + double max_gate_threshold = HUGE_VAL) { MeasurementsZd inside_meas; MeasurementsZd outside_meas; for (const auto& measurement : z_meas) { - double distance = z_pred.mahalanobis_distance(measurement); - - if (distance <= gate_threshold) + double mahalanobis_distance = z_pred.mahalanobis_distance(measurement); + double regular_distance = (z_pred.mean() - measurement).norm(); + if ((mahalanobis_distance <= mahalanobis_threshold || regular_distance <= min_gate_threshold) && + regular_distance <= max_gate_threshold) { inside_meas.push_back(measurement); } diff --git a/vortex-filtering/test/ipda_test.cpp b/vortex-filtering/test/ipda_test.cpp index c6a35a8d..502160c3 100644 --- a/vortex-filtering/test/ipda_test.cpp +++ b/vortex-filtering/test/ipda_test.cpp @@ -8,10 +8,15 @@ using IPDA = vortex::filter::IPDA, vortex::m TEST(IPDA, ipda_runs) { - double gate_threshold = 1.12; - double prob_of_detection = 0.8; - double clutter_intensity = 1.0; - double probability_of_survival = 0.9; + IPDA::Config config; + + config.mahalanobis_threshold = 1.12; + config.min_gate_threshold = 0.0; + config.max_gate_threshold = 100.0; + config.prob_of_detection = 0.8; + config.clutter_intensity = 1.0; + config.prob_of_survival = 0.9; + double last_detection_probability = 0.85; vortex::prob::Gauss4d x_est(Eigen::Vector4d(0.0, 0.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); @@ -22,8 +27,7 @@ TEST(IPDA, ipda_runs) auto sen_model = std::make_shared>(1.0); auto [x_final, existence_pred, inside, outside, x_pred, z_pred, x_updated] = - IPDA::step(x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, probability_of_survival, - last_detection_probability, clutter_intensity); + IPDA::step(dyn_model, sen_model, 1.0, x_est, meas, last_detection_probability, config); std::cout << "Existence probability: " << existence_pred << std::endl; } diff --git a/vortex-filtering/test/pdaf_test.cpp b/vortex-filtering/test/pdaf_test.cpp index e9d24882..459761c1 100644 --- a/vortex-filtering/test/pdaf_test.cpp +++ b/vortex-filtering/test/pdaf_test.cpp @@ -147,25 +147,25 @@ TEST(PDAF, average_state_is_in_between_prediction_and_measurement_both_axes) // testing the apply_gate function TEST(PDAF, apply_gate_is_calculating) { - double gate_threshold = 1.8; + double mahalanobis_threshold = 1.8; vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); std::vector meas = { { 0.0, 1.0 }, { 1.0, 0.0 }, { 1.0, 1.0 }, { 0.0, 2.0 }, { 2.0, 0.0 }, { 2.0, 2.0 } }; - auto [inside, outside] = PDAF::apply_gate(meas, z_pred, gate_threshold); + auto [inside, outside] = PDAF::apply_gate(meas, z_pred, mahalanobis_threshold); } TEST(PDAF, apply_gate_is_separating_correctly) { - double gate_threshold = 3; + double mahalanobis_threshold = 3; Eigen::Matrix2d cov; cov << 1.0, 0.0, 0.0, 4.0; vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), cov); std::vector meas = { { 0.0, 4.0 }, { 4.0, 0.0 } }; - auto [inside, outside] = PDAF::apply_gate(meas, z_pred, gate_threshold); + auto [inside, outside] = PDAF::apply_gate(meas, z_pred, mahalanobis_threshold); EXPECT_EQ(inside.size(), 1u); EXPECT_EQ(outside.size(), 1u); @@ -185,7 +185,7 @@ TEST(PDAF, apply_gate_is_separating_correctly) << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; gp << "replot\n"; - vortex::plotting::Ellipse prediction = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); + vortex::plotting::Ellipse prediction = vortex::plotting::gauss_to_ellipse(z_pred, mahalanobis_threshold); gp << "set object " << ++object_counter << " ellipse center " << prediction.x << "," << prediction.y << " size " << prediction.a << "," << prediction.b << " angle " << prediction.angle << "fs empty border lc rgb 'cyan'\n"; @@ -194,12 +194,12 @@ TEST(PDAF, apply_gate_is_separating_correctly) TEST(PDAF, apply_gate_is_separating_correctly_2) { - double gate_threshold = 2.1; + double mahalanobis_threshold = 2.1; vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); std::vector meas = { { 0.0, 1.0 }, { 1.0, 0.0 }, { 1.0, 1.0 }, { 0.0, 2.0 }, { 2.0, 0.0 }, { 2.0, 2.0 } }; - auto [inside, outside] = PDAF::apply_gate(meas, z_pred, gate_threshold); + auto [inside, outside] = PDAF::apply_gate(meas, z_pred, mahalanobis_threshold); EXPECT_EQ(inside.size(), 5u); EXPECT_EQ(outside.size(), 1u); @@ -217,7 +217,7 @@ TEST(PDAF, apply_gate_is_separating_correctly_2) << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; gp << "replot\n"; - vortex::plotting::Ellipse prediction = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); + vortex::plotting::Ellipse prediction = vortex::plotting::gauss_to_ellipse(z_pred, mahalanobis_threshold); gp << "set object " << ++object_counter << " ellipse center " << prediction.x << "," << prediction.y << " size " << prediction.a << "," << prediction.b << " angle " << prediction.angle << "fs empty border lc rgb 'cyan'\n"; @@ -227,9 +227,10 @@ TEST(PDAF, apply_gate_is_separating_correctly_2) // testing the predict_next_state function TEST(PDAF, predict_next_state_is_calculating) { - double gate_threshold = 1.12; - double prob_of_detection = 0.8; - double clutter_intensity = 1.0; + PDAF::Config config; + config.mahalanobis_threshold = 1.12; + config.prob_of_detection = 0.8; + config.clutter_intensity = 1.0; vortex::prob::Gauss4d x_est(Eigen::Vector4d(0.0, 0.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); std::vector meas = { { 0.0, 1.0 }, { 1.0, 0.0 }, { 1.0, 1.0 }, { 0.0, 2.0 }, { 2.0, 0.0 }, { 2.0, 2.0 } }; @@ -238,7 +239,7 @@ TEST(PDAF, predict_next_state_is_calculating) auto sen_model = std::make_shared>(1.0); auto [x_final, inside, outside, x_pred, z_pred, x_updated] = - PDAF::step(x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); + PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); std::cout << "x_final: " << x_final.mean() << std::endl; Gnuplot gp; @@ -276,7 +277,7 @@ TEST(PDAF, predict_next_state_is_calculating) gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n"; - vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); + vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.mahalanobis_threshold); gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle << " fs empty border lc rgb 'cyan'\n"; @@ -285,9 +286,10 @@ TEST(PDAF, predict_next_state_is_calculating) TEST(PDAF, predict_next_state_2) { - double gate_threshold = 2; - double prob_of_detection = 0.8; - double clutter_intensity = 1.0; + PDAF::Config config; + config.mahalanobis_threshold = 2.0; + config.prob_of_detection = 0.8; + config.clutter_intensity = 1.0; vortex::prob::Gauss4d x_est(Eigen::Vector4d(1.0, 1.5, -2.0, 0.0), Eigen::Matrix4d::Identity()); std::vector meas = { { -3.0, -3.0 }, { 0.0, 0.0 }, { -1.2, 1.5 }, { -2.0, -2.0 }, { 2.0, 0.0 }, { -1.0, 1.0 } }; @@ -296,7 +298,7 @@ TEST(PDAF, predict_next_state_2) auto sen_model = std::make_shared>(0.5); auto [x_final, inside, outside, x_pred, z_pred, x_updated] = - PDAF::step(x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); + PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); std::cout << "x_final: " << x_final.mean() << std::endl; Gnuplot gp; @@ -334,7 +336,7 @@ TEST(PDAF, predict_next_state_2) gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n"; - vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); + vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.mahalanobis_threshold); gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle << " fs empty border lc rgb 'cyan'\n"; @@ -343,9 +345,10 @@ TEST(PDAF, predict_next_state_2) TEST(PDAF, predict_next_state_3_1) { - double gate_threshold = 4; - double prob_of_detection = 0.9; - double clutter_intensity = 1.0; + PDAF::Config config; + config.mahalanobis_threshold = 4.0; + config.prob_of_detection = 0.9; + config.clutter_intensity = 1.0; vortex::prob::Gauss4d x_est(Eigen::Vector4d(0.5, 0.5, -0.75, -0.75), Eigen::Matrix4d::Identity()); std::vector meas = { { 0.0, 0.5 }, { 0.2, 0.2 }, { 0.8, 2.3 }, { 0.5, 0.0 }, { 4.2, 2.7 }, { 1.4, 2.5 } }; @@ -354,7 +357,7 @@ TEST(PDAF, predict_next_state_3_1) auto sen_model = std::make_shared>(1.0); auto [x_final, inside, outside, x_pred, z_pred, x_updated] = - PDAF::step(x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); + PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); std::cout << "x_final: " << x_final.mean() << std::endl; Gnuplot gp; @@ -394,7 +397,7 @@ TEST(PDAF, predict_next_state_3_1) gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n"; - vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); + vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.mahalanobis_threshold); gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle << " fs empty border lc rgb 'cyan'\n"; @@ -403,9 +406,10 @@ TEST(PDAF, predict_next_state_3_1) TEST(PDAF, predict_next_state_3_2) { - double gate_threshold = 4; - double prob_of_detection = 0.9; - double clutter_intensity = 1.0; + PDAF::Config config; + config.mahalanobis_threshold = 4.0; + config.prob_of_detection = 0.9; + config.clutter_intensity = 1.0; vortex::prob::Gauss4d x_est(Eigen::Vector4d(-0.00173734, 0.0766262, -0.614584, -0.57184), Eigen::Matrix4d::Identity()); std::vector meas = { { -0.5, 2.0 }, { -0.23, 0.5 }, { -2.0, 3.4 }, @@ -415,7 +419,7 @@ TEST(PDAF, predict_next_state_3_2) auto sen_model = std::make_shared>(1.0); auto [x_final, inside, outside, x_pred, z_pred, x_updated] = - PDAF::step(x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); + PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); std::cout << "x_final: " << x_final.mean() << std::endl; Gnuplot gp; @@ -459,7 +463,7 @@ TEST(PDAF, predict_next_state_3_2) gp << "set arrow from " << 0.5 << "," << 0.5 << " to " << -0.00173734 << "," << 0.0766262 << " nohead lc rgb 'orange-red'\n"; - vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); + vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.mahalanobis_threshold); gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle << " fs empty border lc rgb 'cyan'\n"; @@ -468,9 +472,10 @@ TEST(PDAF, predict_next_state_3_2) TEST(PDAF, predict_next_state_3_3) { - double gate_threshold = 4; - double prob_of_detection = 0.9; - double clutter_intensity = 1.0; + PDAF::Config config; + config.mahalanobis_threshold = 4.0; + config.prob_of_detection = 0.9; + config.clutter_intensity = 1.0; vortex::prob::Gauss4d x_est(Eigen::Vector4d(-0.55929, 0.0694888, -0.583476, -0.26382), Eigen::Matrix4d::Identity()); std::vector meas = { { -1.5, 2.5 }, { -1.2, 2.7 }, { -0.8, 2.3 }, { -1.7, 1.9 }, { -2.0, 3.0 }, { -1.11, 2.04 } }; @@ -479,7 +484,7 @@ TEST(PDAF, predict_next_state_3_3) auto sen_model = std::make_shared>(1.0); auto [x_final, inside, outside, x_pred, z_pred, x_updated] = - PDAF::step(x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); + PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); std::cout << "x_final: " << x_final.mean() << std::endl; Gnuplot gp; @@ -527,7 +532,7 @@ TEST(PDAF, predict_next_state_3_3) gp << "set arrow from " << -0.00173734 << "," << 0.0766262 << " to " << -0.55929 << "," << 0.0694888 << " nohead lc rgb 'orange-red'\n"; - vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); + vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.mahalanobis_threshold); gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle << " fs empty border lc rgb 'cyan'\n"; @@ -536,9 +541,10 @@ TEST(PDAF, predict_next_state_3_3) TEST(PDAF, predict_next_state_3_4) { - double gate_threshold = 4; - double prob_of_detection = 0.9; - double clutter_intensity = 1.0; + PDAF::Config config; + config.mahalanobis_threshold = 4.0; + config.prob_of_detection = 0.9; + config.clutter_intensity = 1.0; vortex::prob::Gauss4d x_est(Eigen::Vector4d(-1.20613, 0.610616, -0.618037, 0.175242), Eigen::Matrix4d::Identity()); std::vector meas = { { -2.0, 2.2 }, { -1.8, 2.3 }, { -2.3, 2.0 }, { 0.6, 1.5 }, { -2.0, 2.0 }, { -1.4, 2.5 } }; @@ -547,7 +553,7 @@ TEST(PDAF, predict_next_state_3_4) auto sen_model = std::make_shared>(1.0); auto [x_final, inside, outside, x_pred, z_pred, x_updated] = - PDAF::step(x_est, meas, 1.0, dyn_model, sen_model, gate_threshold, prob_of_detection, clutter_intensity); + PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); std::cout << "x_final: " << x_final.mean() << std::endl; Gnuplot gp; @@ -599,7 +605,7 @@ TEST(PDAF, predict_next_state_3_4) gp << "set arrow from " << -0.55929 << "," << 0.0694888 << " to " << -1.20613 << "," << 0.610616 << " nohead lc rgb 'orange-red'\n"; - vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold); + vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.mahalanobis_threshold); gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle << " fs empty border lc rgb 'cyan'\n"; From 68708dd9dadd7a5da5f3d3f2160178d079e675ff Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Eirik=20Kol=C3=A5s?= Date: Mon, 18 Mar 2024 14:47:15 +0100 Subject: [PATCH 23/31] refactored pdaf to work with new type system --- .../include/vortex_filtering/filters/ipda.hpp | 38 ++--- .../include/vortex_filtering/filters/pdaf.hpp | 99 +++++------- vortex-filtering/test/CMakeLists.txt | 2 + vortex-filtering/test/ipda_test.cpp | 10 +- vortex-filtering/test/pdaf_test.cpp | 144 +++++++++--------- 5 files changed, 139 insertions(+), 154 deletions(-) diff --git a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp index d1368708..6e3a180b 100644 --- a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp @@ -7,20 +7,24 @@ namespace vortex::filter { -template -class IPDA -{ +template class IPDA { public: - using SensModI = typename SensModT::SensModI; - using DynModI = typename DynModT::DynModI; - using DynModPtr = std::shared_ptr; - using SensModPtr = std::shared_ptr; - using EKF = vortex::filter::EKF; - using Gauss_z = typename SensModI::Gauss_z; - using Gauss_x = typename DynModI::Gauss_x; - using Vec_z = typename SensModI::Vec_z; - using GaussMix = vortex::prob::GaussianMixture; - using PDAF = vortex::filter::PDAF, vortex::models::IdentitySensorModel<4, 2>>; + static constexpr int N_DIM_x = DynModT::N_DIM_x; + static constexpr int N_DIM_z = SensModT::N_DIM_z; + static constexpr int N_DIM_u = DynModT::N_DIM_u; + static constexpr int N_DIM_v = DynModT::N_DIM_v; + static constexpr int N_DIM_w = SensModT::N_DIM_w; + + using T = Types_xzuvw; + + using EKF = vortex::filter::EKF_t; + + using Gauss_z = typename T::Gauss_z; + using Gauss_x = typename T::Gauss_x; + using Vec_z = typename T::Vec_z; + using GaussMix = vortex::prob::GaussianMixture; + using PDAF = vortex::filter::PDAF>; + IPDA() = delete; struct Config : public PDAF::Config @@ -41,18 +45,18 @@ class IPDA { double predicted_existence_probability = probability_of_survival * last_detection_probability_; // Finds Ps - double summed = 0; + double sum = 0.0; for (const Vec_z& measurement : measurements) { - summed += z_pred.pdf(measurement); + sum += z_pred.pdf(measurement); } - double l_k = 1 - probability_of_detection + probability_of_detection / clutter_intensity * summed; + double l_k = 1 - probability_of_detection + probability_of_detection / clutter_intensity * sum; return (l_k * predicted_existence_probability) / (1 - (1 - l_k) * predicted_existence_probability); } static std::tuple, std::vector, Gauss_x, Gauss_z, std::vector> - step(const DynModPtr& dyn_model, const SensModPtr& sen_model, double timestep, const Gauss_x& x_est, + step(const DynModT& dyn_model, const SensModT& sen_model, double timestep, const Gauss_x& x_est, const std::vector& z_meas, double survive_est, const IPDA::Config& config) { auto [x_final, inside, outside, x_pred, z_pred, x_updated] = diff --git a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp index 3a2a6fbb..5d838134 100644 --- a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp @@ -7,90 +7,74 @@ #include #include -namespace vortex -{ -namespace filter -{ +namespace vortex::filter { -using std::string; -// using std::endl; - -template -class PDAF -{ +template class PDAF { public: - using SensModI = typename SensModT::SensModI; - using DynModI = typename DynModT::DynModI; - using DynModPtr = std::shared_ptr; - using SensModPtr = std::shared_ptr; - using EKF = vortex::filter::EKF; - using Gauss_z = typename SensModI::Gauss_z; - using Gauss_x = typename DynModI::Gauss_x; - using Vec_z = typename SensModI::Vec_z; + static constexpr int N_DIM_x = DynModT::N_DIM_x; + static constexpr int N_DIM_z = SensModT::N_DIM_z; + static constexpr int N_DIM_u = DynModT::N_DIM_u; + static constexpr int N_DIM_v = DynModT::N_DIM_v; + static constexpr int N_DIM_w = SensModT::N_DIM_w; + + using T = Types_xzuvw; + + using EKF = vortex::filter::EKF_t; + using Gauss_z = typename T::Gauss_z; + using Gauss_x = typename T::Gauss_x; + using Vec_z = typename T::Vec_z; using MeasurementsZd = std::vector; - using StatesXd = std::vector; - using GaussMixZd = vortex::prob::GaussianMixture; + using StatesXd = std::vector; + using GaussMixZd = vortex::prob::GaussianMixture; - struct Config - { + struct Config { double mahalanobis_threshold = 1.0; - double min_gate_threshold = 0.0; - double max_gate_threshold = HUGE_VAL; - double prob_of_detection = 1.0; - double clutter_intensity = 1.0; + double min_gate_threshold = 0.0; + double max_gate_threshold = HUGE_VAL; + double prob_of_detection = 1.0; + double clutter_intensity = 1.0; }; PDAF() = delete; static std::tuple - step(const DynModPtr& dyn_model, const SensModPtr& sen_model, double timestep, const Gauss_x& x_est, - const MeasurementsZd& z_meas, const Config& config) + step(const DynModT &dyn_model, const SensModT &sen_model, double timestep, const Gauss_x &x_est, const MeasurementsZd &z_meas, const Config &config) { - auto [x_pred, z_pred] = EKF::predict(dyn_model, sen_model, timestep, x_est); - auto [inside, outside] = - apply_gate(z_meas, z_pred, config.mahalanobis_threshold, config.min_gate_threshold, config.max_gate_threshold); + auto [x_pred, z_pred] = EKF::predict(dyn_model, sen_model, timestep, x_est); + auto [inside, outside] = apply_gate(z_meas, z_pred, config.mahalanobis_threshold, config.min_gate_threshold, config.max_gate_threshold); StatesXd x_updated; - for (const auto& measurement : inside) - { + for (const auto &measurement : inside) { x_updated.push_back(EKF::update(sen_model, x_pred, z_pred, measurement)); } - Gauss_x x_final = - get_weighted_average(inside, x_updated, z_pred, x_pred, config.prob_of_detection, config.clutter_intensity); - return { x_final, inside, outside, x_pred, z_pred, x_updated }; + Gauss_x x_final = get_weighted_average(inside, x_updated, z_pred, x_pred, config.prob_of_detection, config.clutter_intensity); + return {x_final, inside, outside, x_pred, z_pred, x_updated}; } - static std::tuple apply_gate(const MeasurementsZd& z_meas, const Gauss_z& z_pred, - double mahalanobis_threshold, - double min_gate_threshold = 0.0, - double max_gate_threshold = HUGE_VAL) + static std::tuple apply_gate(const MeasurementsZd &z_meas, const Gauss_z &z_pred, double mahalanobis_threshold, + double min_gate_threshold = 0.0, double max_gate_threshold = HUGE_VAL) { MeasurementsZd inside_meas; MeasurementsZd outside_meas; - for (const auto& measurement : z_meas) - { + for (const auto &measurement : z_meas) { double mahalanobis_distance = z_pred.mahalanobis_distance(measurement); - double regular_distance = (z_pred.mean() - measurement).norm(); - if ((mahalanobis_distance <= mahalanobis_threshold || regular_distance <= min_gate_threshold) && - regular_distance <= max_gate_threshold) - { + double regular_distance = (z_pred.mean() - measurement).norm(); + if ((mahalanobis_distance <= mahalanobis_threshold || regular_distance <= min_gate_threshold) && regular_distance <= max_gate_threshold) { inside_meas.push_back(measurement); } - else - { + else { outside_meas.push_back(measurement); } } - return { inside_meas, outside_meas }; + return {inside_meas, outside_meas}; } // Getting weighted average of the predicted states - static Gauss_x get_weighted_average(const MeasurementsZd& z_meas, const StatesXd& updated_states, - const Gauss_z& z_pred, const Gauss_x& x_pred, double prob_of_detection, - double clutter_intensity) + static Gauss_x get_weighted_average(const MeasurementsZd &z_meas, const StatesXd &updated_states, const Gauss_z &z_pred, const Gauss_x &x_pred, + double prob_of_detection, double clutter_intensity) { StatesXd states; states.push_back(x_pred); @@ -104,18 +88,16 @@ class PDAF } // Getting association probabilities according to textbook p. 123 "Corollary 7.3.3" - static Eigen::VectorXd get_weights(const MeasurementsZd& z_meas, const Gauss_z& z_pred, double prob_of_detection, - double clutter_intensity) + static Eigen::VectorXd get_weights(const MeasurementsZd &z_meas, const Gauss_z &z_pred, double prob_of_detection, double clutter_intensity) { Eigen::VectorXd weights(z_meas.size() + 1); // in case no measurement assosiates with the target double no_association = clutter_intensity * (1 - prob_of_detection); - weights(0) = no_association; + weights(0) = no_association; // measurements associating with the target - for (size_t k = 1; k < z_meas.size() + 1; k++) - { + for (size_t k = 1; k < z_meas.size() + 1; k++) { weights(k) = (prob_of_detection * z_pred.pdf(z_meas.at(k - 1))); } @@ -126,5 +108,4 @@ class PDAF } }; -} // namespace filter -} // namespace vortex \ No newline at end of file +} // namespace vortex::filter \ No newline at end of file diff --git a/vortex-filtering/test/CMakeLists.txt b/vortex-filtering/test/CMakeLists.txt index 14d34b09..cb1379bb 100644 --- a/vortex-filtering/test/CMakeLists.txt +++ b/vortex-filtering/test/CMakeLists.txt @@ -8,7 +8,9 @@ ament_add_gtest(${PROJECT_NAME}_test dynamic_model_test.cpp ekf_test.cpp imm_test.cpp + ipda_test.cpp numerical_integration_test.cpp + pdaf_test.cpp probability_test.cpp sensor_model_test.cpp types_test.cpp diff --git a/vortex-filtering/test/ipda_test.cpp b/vortex-filtering/test/ipda_test.cpp index 502160c3..120d620c 100644 --- a/vortex-filtering/test/ipda_test.cpp +++ b/vortex-filtering/test/ipda_test.cpp @@ -2,9 +2,11 @@ #include #include #include -#include +#include -using IPDA = vortex::filter::IPDA, vortex::models::IdentitySensorModel<4, 2>>; +using ConstantVelocity = vortex::models::ConstantVelocity; +using IdentitySensorModel = vortex::models::IdentitySensorModel<4, 2>; +using IPDA = vortex::filter::IPDA; TEST(IPDA, ipda_runs) { @@ -23,8 +25,8 @@ TEST(IPDA, ipda_runs) std::vector meas = { { 0.0, 1.0 }, { 1.0, 0.0 }, { 1.0, 1.0 }, { 0.0, 2.0 }, { 2.0, 0.0 }, { 2.0, 2.0 } }; - auto dyn_model = std::make_shared>(1.0); - auto sen_model = std::make_shared>(1.0); + ConstantVelocity dyn_model{1.0}; + IdentitySensorModel sen_model{1.0}; auto [x_final, existence_pred, inside, outside, x_pred, z_pred, x_updated] = IPDA::step(dyn_model, sen_model, 1.0, x_est, meas, last_detection_probability, config); diff --git a/vortex-filtering/test/pdaf_test.cpp b/vortex-filtering/test/pdaf_test.cpp index 459761c1..9f078b93 100644 --- a/vortex-filtering/test/pdaf_test.cpp +++ b/vortex-filtering/test/pdaf_test.cpp @@ -2,9 +2,11 @@ #include #include #include -#include +#include -using PDAF = vortex::filter::PDAF, vortex::models::IdentitySensorModel<4, 2>>; +using ConstantVelocity = vortex::models::ConstantVelocity; +using IdentitySensorModel = vortex::models::IdentitySensorModel<4, 2>; +using PDAF = vortex::filter::PDAF; // testing the get_weights function TEST(PDAF, get_weights_is_calculating) @@ -172,6 +174,7 @@ TEST(PDAF, apply_gate_is_separating_correctly) EXPECT_EQ(inside[0], meas[0]); EXPECT_EQ(outside[0], meas[1]); +#if (GNUPLOT_ENABLE) Gnuplot gp; gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; gp << "set size ratio -1\n"; @@ -185,11 +188,12 @@ TEST(PDAF, apply_gate_is_separating_correctly) << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; gp << "replot\n"; - vortex::plotting::Ellipse prediction = vortex::plotting::gauss_to_ellipse(z_pred, mahalanobis_threshold); + vortex::utils::Ellipse prediction = vortex::plotting::gauss_to_ellipse(z_pred, mahalanobis_threshold); - gp << "set object " << ++object_counter << " ellipse center " << prediction.x << "," << prediction.y << " size " - << prediction.a << "," << prediction.b << " angle " << prediction.angle << "fs empty border lc rgb 'cyan'\n"; + gp << "set object " << ++object_counter << " ellipse center " << prediction.x() << "," << prediction.y() << " size " << prediction.major_axis() << "," + << prediction.minor_axis() << " angle " << prediction.angle_deg() << "fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; +#endif } TEST(PDAF, apply_gate_is_separating_correctly_2) @@ -204,6 +208,7 @@ TEST(PDAF, apply_gate_is_separating_correctly_2) EXPECT_EQ(inside.size(), 5u); EXPECT_EQ(outside.size(), 1u); +#if (GNUPLOT_ENABLE) Gnuplot gp; gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; gp << "set size ratio -1\n"; @@ -217,11 +222,12 @@ TEST(PDAF, apply_gate_is_separating_correctly_2) << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; gp << "replot\n"; - vortex::plotting::Ellipse prediction = vortex::plotting::gauss_to_ellipse(z_pred, mahalanobis_threshold); + vortex::utils::Ellipse prediction = vortex::plotting::gauss_to_ellipse(z_pred, mahalanobis_threshold); - gp << "set object " << ++object_counter << " ellipse center " << prediction.x << "," << prediction.y << " size " - << prediction.a << "," << prediction.b << " angle " << prediction.angle << "fs empty border lc rgb 'cyan'\n"; + gp << "set object " << ++object_counter << " ellipse center " << prediction.x() << "," << prediction.y() << " size " << prediction.major_axis() << "," + << prediction.minor_axis() << " angle " << prediction.angle_deg() << "fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; +#endif } // testing the predict_next_state function @@ -235,13 +241,14 @@ TEST(PDAF, predict_next_state_is_calculating) std::vector meas = { { 0.0, 1.0 }, { 1.0, 0.0 }, { 1.0, 1.0 }, { 0.0, 2.0 }, { 2.0, 0.0 }, { 2.0, 2.0 } }; - auto dyn_model = std::make_shared>(1.0); - auto sen_model = std::make_shared>(1.0); + ConstantVelocity dyn_model{1.0}; + IdentitySensorModel sen_model{1.0}; auto [x_final, inside, outside, x_pred, z_pred, x_updated] = PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); std::cout << "x_final: " << x_final.mean() << std::endl; +#if (GNUPLOT_ENABLE) Gnuplot gp; gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; gp << "set size ratio -1\n"; @@ -255,14 +262,12 @@ TEST(PDAF, predict_next_state_is_calculating) for (const auto& state : x_updated) { vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2, 2)); - vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); + vortex::utils::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); - gp << "set object " << ++object_counter << " ellipse center " << ellipse.x << "," << ellipse.y << " size " - << ellipse.a << "," << ellipse.b << " angle " << ellipse.angle - << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + gp << "set object " << ++object_counter << " ellipse center " << ellipse.x() << "," << ellipse.y() << " size " << ellipse.major_axis() << "," + << ellipse.minor_axis() << " angle " << ellipse.angle_deg() << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; - gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 - << " fs empty border lc rgb 'blue'\n"; + gp << "set object " << ++object_counter << " circle center " << ellipse.x() << "," << ellipse.y() << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; } gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " @@ -277,11 +282,12 @@ TEST(PDAF, predict_next_state_is_calculating) gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n"; - vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.mahalanobis_threshold); - gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," - << gate.b << " angle " << gate.angle << " fs empty border lc rgb 'cyan'\n"; + vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.mahalanobis_threshold); + gp << "set object " << ++object_counter << " ellipse center " << gate.x() << "," << gate.y() << " size " << gate.major_axis() << "," << gate.minor_axis() + << " angle " << gate.angle_deg() << " fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; +#endif } TEST(PDAF, predict_next_state_2) @@ -294,8 +300,8 @@ TEST(PDAF, predict_next_state_2) std::vector meas = { { -3.0, -3.0 }, { 0.0, 0.0 }, { -1.2, 1.5 }, { -2.0, -2.0 }, { 2.0, 0.0 }, { -1.0, 1.0 } }; - auto dyn_model = std::make_shared>(1.0); - auto sen_model = std::make_shared>(0.5); + ConstantVelocity dyn_model{1.0}; + IdentitySensorModel sen_model{0.5}; auto [x_final, inside, outside, x_pred, z_pred, x_updated] = PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); @@ -314,14 +320,12 @@ TEST(PDAF, predict_next_state_2) for (const auto& state : x_updated) { vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2, 2)); - vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); + vortex::utils::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); - gp << "set object " << ++object_counter << " ellipse center " << ellipse.x << "," << ellipse.y << " size " - << ellipse.a << "," << ellipse.b << " angle " << ellipse.angle - << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + gp << "set object " << ++object_counter << " ellipse center " << ellipse.x() << "," << ellipse.y() << " size " << ellipse.major_axis() << "," + << ellipse.minor_axis() << " angle " << ellipse.angle_deg() << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; - gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 - << " fs empty border lc rgb 'blue'\n"; + gp << "set object " << ++object_counter << " circle center " << ellipse.x() << "," << ellipse.y() << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; } gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " @@ -336,9 +340,9 @@ TEST(PDAF, predict_next_state_2) gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n"; - vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.mahalanobis_threshold); - gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," - << gate.b << " angle " << gate.angle << " fs empty border lc rgb 'cyan'\n"; + vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.mahalanobis_threshold); + gp << "set object " << ++object_counter << " ellipse center " << gate.x() << "," << gate.y() << " size " << gate.major_axis() << "," << gate.minor_axis() + << " angle " << gate.angle_deg() << " fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; } @@ -353,8 +357,8 @@ TEST(PDAF, predict_next_state_3_1) std::vector meas = { { 0.0, 0.5 }, { 0.2, 0.2 }, { 0.8, 2.3 }, { 0.5, 0.0 }, { 4.2, 2.7 }, { 1.4, 2.5 } }; - auto dyn_model = std::make_shared>(0.5); - auto sen_model = std::make_shared>(1.0); + ConstantVelocity dyn_model{0.5}; + IdentitySensorModel sen_model{1.0}; auto [x_final, inside, outside, x_pred, z_pred, x_updated] = PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); @@ -373,14 +377,12 @@ TEST(PDAF, predict_next_state_3_1) for (const auto& state : x_updated) { vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2, 2)); - vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); + vortex::utils::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); - gp << "set object " << ++object_counter << " ellipse center " << ellipse.x << "," << ellipse.y << " size " - << ellipse.a << "," << ellipse.b << " angle " << ellipse.angle - << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + gp << "set object " << ++object_counter << " ellipse center " << ellipse.x() << "," << ellipse.y() << " size " << ellipse.major_axis() << "," + << ellipse.minor_axis() << " angle " << ellipse.angle_deg() << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; - gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 - << " fs empty border lc rgb 'blue'\n"; + gp << "set object " << ++object_counter << " circle center " << ellipse.x() << "," << ellipse.y() << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; } gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " @@ -397,9 +399,9 @@ TEST(PDAF, predict_next_state_3_1) gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n"; - vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.mahalanobis_threshold); - gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," - << gate.b << " angle " << gate.angle << " fs empty border lc rgb 'cyan'\n"; + vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.mahalanobis_threshold); + gp << "set object " << ++object_counter << " ellipse center " << gate.x() << "," << gate.y() << " size " << gate.major_axis() << "," << gate.minor_axis() + << " angle " << gate.angle_deg() << " fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; } @@ -415,8 +417,8 @@ TEST(PDAF, predict_next_state_3_2) std::vector meas = { { -0.5, 2.0 }, { -0.23, 0.5 }, { -2.0, 3.4 }, { 0.0, 1.3 }, { 0.14, 0.5 }, { -2.5, 0.89 } }; - auto dyn_model = std::make_shared>(0.5); - auto sen_model = std::make_shared>(1.0); + ConstantVelocity dyn_model{0.5}; + IdentitySensorModel sen_model{1.0}; auto [x_final, inside, outside, x_pred, z_pred, x_updated] = PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); @@ -435,14 +437,12 @@ TEST(PDAF, predict_next_state_3_2) for (const auto& state : x_updated) { vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2, 2)); - vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); + vortex::utils::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); - gp << "set object " << ++object_counter << " ellipse center " << ellipse.x << "," << ellipse.y << " size " - << ellipse.a << "," << ellipse.b << " angle " << ellipse.angle - << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + gp << "set object " << ++object_counter << " ellipse center " << ellipse.x() << "," << ellipse.y() << " size " << ellipse.major_axis() << "," + << ellipse.minor_axis() << " angle " << ellipse.angle_deg() << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; - gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 - << " fs empty border lc rgb 'blue'\n"; + gp << "set object " << ++object_counter << " circle center " << ellipse.x() << "," << ellipse.y() << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; } gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " @@ -463,9 +463,9 @@ TEST(PDAF, predict_next_state_3_2) gp << "set arrow from " << 0.5 << "," << 0.5 << " to " << -0.00173734 << "," << 0.0766262 << " nohead lc rgb 'orange-red'\n"; - vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.mahalanobis_threshold); - gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," - << gate.b << " angle " << gate.angle << " fs empty border lc rgb 'cyan'\n"; + vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.mahalanobis_threshold); + gp << "set object " << ++object_counter << " ellipse center " << gate.x() << "," << gate.y() << " size " << gate.major_axis() << "," << gate.minor_axis() + << " angle " << gate.angle_deg() << " fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; } @@ -480,8 +480,8 @@ TEST(PDAF, predict_next_state_3_3) std::vector meas = { { -1.5, 2.5 }, { -1.2, 2.7 }, { -0.8, 2.3 }, { -1.7, 1.9 }, { -2.0, 3.0 }, { -1.11, 2.04 } }; - auto dyn_model = std::make_shared>(0.5); - auto sen_model = std::make_shared>(1.0); + ConstantVelocity dyn_model{0.5}; + IdentitySensorModel sen_model{1.0}; auto [x_final, inside, outside, x_pred, z_pred, x_updated] = PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); @@ -500,14 +500,12 @@ TEST(PDAF, predict_next_state_3_3) for (const auto& state : x_updated) { vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2, 2)); - vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); + vortex::utils::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); - gp << "set object " << ++object_counter << " ellipse center " << ellipse.x << "," << ellipse.y << " size " - << ellipse.a << "," << ellipse.b << " angle " << ellipse.angle - << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + gp << "set object " << ++object_counter << " ellipse center " << ellipse.x() << "," << ellipse.y() << " size " << ellipse.major_axis() << "," + << ellipse.minor_axis() << " angle " << ellipse.angle_deg() << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; - gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 - << " fs empty border lc rgb 'blue'\n"; + gp << "set object " << ++object_counter << " circle center " << ellipse.x() << "," << ellipse.y() << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; } gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " @@ -532,9 +530,9 @@ TEST(PDAF, predict_next_state_3_3) gp << "set arrow from " << -0.00173734 << "," << 0.0766262 << " to " << -0.55929 << "," << 0.0694888 << " nohead lc rgb 'orange-red'\n"; - vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.mahalanobis_threshold); - gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," - << gate.b << " angle " << gate.angle << " fs empty border lc rgb 'cyan'\n"; + vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.mahalanobis_threshold); + gp << "set object " << ++object_counter << " ellipse center " << gate.x() << "," << gate.y() << " size " << gate.major_axis() << "," << gate.minor_axis() + << " angle " << gate.angle_deg() << " fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; } @@ -549,8 +547,8 @@ TEST(PDAF, predict_next_state_3_4) std::vector meas = { { -2.0, 2.2 }, { -1.8, 2.3 }, { -2.3, 2.0 }, { 0.6, 1.5 }, { -2.0, 2.0 }, { -1.4, 2.5 } }; - auto dyn_model = std::make_shared>(0.5); - auto sen_model = std::make_shared>(1.0); + ConstantVelocity dyn_model{0.5}; + IdentitySensorModel sen_model{1.0}; auto [x_final, inside, outside, x_pred, z_pred, x_updated] = PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); @@ -569,14 +567,12 @@ TEST(PDAF, predict_next_state_3_4) for (const auto& state : x_updated) { vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2, 2)); - vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); + vortex::utils::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); - gp << "set object " << ++object_counter << " ellipse center " << ellipse.x << "," << ellipse.y << " size " - << ellipse.a << "," << ellipse.b << " angle " << ellipse.angle - << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + gp << "set object " << ++object_counter << " ellipse center " << ellipse.x() << "," << ellipse.y() << " size " << ellipse.major_axis() << "," + << ellipse.minor_axis() << " angle " << ellipse.angle_deg() << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; - gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 - << " fs empty border lc rgb 'blue'\n"; + gp << "set object " << ++object_counter << " circle center " << ellipse.x() << "," << ellipse.y() << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; } gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " @@ -605,9 +601,9 @@ TEST(PDAF, predict_next_state_3_4) gp << "set arrow from " << -0.55929 << "," << 0.0694888 << " to " << -1.20613 << "," << 0.610616 << " nohead lc rgb 'orange-red'\n"; - vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.mahalanobis_threshold); - gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," - << gate.b << " angle " << gate.angle << " fs empty border lc rgb 'cyan'\n"; + vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.mahalanobis_threshold); + gp << "set object " << ++object_counter << " ellipse center " << gate.x() << "," << gate.y() << " size " << gate.major_axis() << "," << gate.minor_axis() + << " angle " << gate.angle_deg() << " fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; } \ No newline at end of file From 753e4aa082316409bc272d7c86683da5e58b774e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Eirik=20Kol=C3=A5s?= Date: Mon, 18 Mar 2024 15:29:56 +0100 Subject: [PATCH 24/31] hid plotting inside #define clause --- vortex-filtering/test/pdaf_test.cpp | 22 ++++++++++++++++------ 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/vortex-filtering/test/pdaf_test.cpp b/vortex-filtering/test/pdaf_test.cpp index 9f078b93..23844797 100644 --- a/vortex-filtering/test/pdaf_test.cpp +++ b/vortex-filtering/test/pdaf_test.cpp @@ -174,7 +174,7 @@ TEST(PDAF, apply_gate_is_separating_correctly) EXPECT_EQ(inside[0], meas[0]); EXPECT_EQ(outside[0], meas[1]); -#if (GNUPLOT_ENABLE) + #if (GNUPLOT_ENABLE) Gnuplot gp; gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; gp << "set size ratio -1\n"; @@ -193,7 +193,7 @@ TEST(PDAF, apply_gate_is_separating_correctly) gp << "set object " << ++object_counter << " ellipse center " << prediction.x() << "," << prediction.y() << " size " << prediction.major_axis() << "," << prediction.minor_axis() << " angle " << prediction.angle_deg() << "fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; -#endif + #endif } TEST(PDAF, apply_gate_is_separating_correctly_2) @@ -208,7 +208,7 @@ TEST(PDAF, apply_gate_is_separating_correctly_2) EXPECT_EQ(inside.size(), 5u); EXPECT_EQ(outside.size(), 1u); -#if (GNUPLOT_ENABLE) + #if (GNUPLOT_ENABLE) Gnuplot gp; gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; gp << "set size ratio -1\n"; @@ -227,7 +227,7 @@ TEST(PDAF, apply_gate_is_separating_correctly_2) gp << "set object " << ++object_counter << " ellipse center " << prediction.x() << "," << prediction.y() << " size " << prediction.major_axis() << "," << prediction.minor_axis() << " angle " << prediction.angle_deg() << "fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; -#endif + #endif } // testing the predict_next_state function @@ -248,7 +248,7 @@ TEST(PDAF, predict_next_state_is_calculating) PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); std::cout << "x_final: " << x_final.mean() << std::endl; -#if (GNUPLOT_ENABLE) + #if (GNUPLOT_ENABLE) Gnuplot gp; gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; gp << "set size ratio -1\n"; @@ -287,7 +287,7 @@ TEST(PDAF, predict_next_state_is_calculating) << " angle " << gate.angle_deg() << " fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; -#endif + #endif } TEST(PDAF, predict_next_state_2) @@ -307,6 +307,7 @@ TEST(PDAF, predict_next_state_2) PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); std::cout << "x_final: " << x_final.mean() << std::endl; + #if (GNUPLOT_ENABLE) Gnuplot gp; gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; gp << "set size ratio -1\n"; @@ -345,6 +346,7 @@ TEST(PDAF, predict_next_state_2) << " angle " << gate.angle_deg() << " fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; + #endif } TEST(PDAF, predict_next_state_3_1) @@ -364,6 +366,7 @@ TEST(PDAF, predict_next_state_3_1) PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); std::cout << "x_final: " << x_final.mean() << std::endl; + #if (GNUPLOT_ENABLE) Gnuplot gp; gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; gp << "set size ratio -1\n"; @@ -404,6 +407,7 @@ TEST(PDAF, predict_next_state_3_1) << " angle " << gate.angle_deg() << " fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; + #endif } TEST(PDAF, predict_next_state_3_2) @@ -424,6 +428,7 @@ TEST(PDAF, predict_next_state_3_2) PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); std::cout << "x_final: " << x_final.mean() << std::endl; + #if (GNUPLOT_ENABLE) Gnuplot gp; gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; gp << "set size ratio -1\n"; @@ -468,6 +473,7 @@ TEST(PDAF, predict_next_state_3_2) << " angle " << gate.angle_deg() << " fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; + #endif } TEST(PDAF, predict_next_state_3_3) @@ -487,6 +493,7 @@ TEST(PDAF, predict_next_state_3_3) PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); std::cout << "x_final: " << x_final.mean() << std::endl; + #if (GNUPLOT_ENABLE) Gnuplot gp; gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; gp << "set size ratio -1\n"; @@ -535,6 +542,7 @@ TEST(PDAF, predict_next_state_3_3) << " angle " << gate.angle_deg() << " fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; + #endif } TEST(PDAF, predict_next_state_3_4) @@ -554,6 +562,7 @@ TEST(PDAF, predict_next_state_3_4) PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); std::cout << "x_final: " << x_final.mean() << std::endl; + #if (GNUPLOT_ENABLE) Gnuplot gp; gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; gp << "set size ratio -1\n"; @@ -606,4 +615,5 @@ TEST(PDAF, predict_next_state_3_4) << " angle " << gate.angle_deg() << " fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; + #endif } \ No newline at end of file From 1ef37d98a506b15fd3020a184f5e272392c6f41b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Eirik=20Kol=C3=A5s?= Date: Sat, 23 Mar 2024 13:01:30 +0100 Subject: [PATCH 25/31] cmake --- vortex-filtering/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/vortex-filtering/CMakeLists.txt b/vortex-filtering/CMakeLists.txt index c4211fd3..437f9c79 100644 --- a/vortex-filtering/CMakeLists.txt +++ b/vortex-filtering/CMakeLists.txt @@ -81,6 +81,7 @@ install(TARGETS ${PROJECT_NAME}_lib # === Export libraries === ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(eigen3_cmake_module) ament_export_dependencies(${lib_deps}) # Export the dependencies so the package using this package can find them From d9a64daace7e54220c8604557e909ae2616c8856 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Eirik=20Kol=C3=A5s?= Date: Sat, 23 Mar 2024 13:03:45 +0100 Subject: [PATCH 26/31] cmake --- vortex-filtering/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/vortex-filtering/CMakeLists.txt b/vortex-filtering/CMakeLists.txt index 437f9c79..cc6ba5f4 100644 --- a/vortex-filtering/CMakeLists.txt +++ b/vortex-filtering/CMakeLists.txt @@ -24,6 +24,7 @@ find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(Eigen3 REQUIRED) +find_package(eigen3_cmake_module REQUIRED) find_package(OpenMP REQUIRED) # For parallel processing with Eigen find_package(Gnuplot REQUIRED) From 362f9edbd0750de4b612b446364b85882ca5b8ca Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Eirik=20Kol=C3=A5s?= Date: Sat, 23 Mar 2024 13:04:44 +0100 Subject: [PATCH 27/31] update package.xml --- vortex-filtering/package.xml | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/vortex-filtering/package.xml b/vortex-filtering/package.xml index 63a7be90..2c9080bb 100644 --- a/vortex-filtering/package.xml +++ b/vortex-filtering/package.xml @@ -8,17 +8,23 @@ MIT ament_cmake + eigen3_cmake_module + eigen rclcpp std_msgs geometry_msgs - eigen + ament_lint_auto ament_lint_common ament_cmake_gtest gnuplot gnuplot-iostream + + eigen3_cmake_module + eigen + ament_cmake From c3593d81d8632e4e6b8c8d9467d925dbb5c9408c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Eirik=20Kol=C3=A5s?= Date: Mon, 25 Mar 2024 12:42:22 +0100 Subject: [PATCH 28/31] update cmake --- vortex-filtering/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/vortex-filtering/CMakeLists.txt b/vortex-filtering/CMakeLists.txt index cc6ba5f4..b49d0ea3 100644 --- a/vortex-filtering/CMakeLists.txt +++ b/vortex-filtering/CMakeLists.txt @@ -23,7 +23,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(geometry_msgs REQUIRED) -find_package(Eigen3 REQUIRED) +find_package(Eigen3 3.4.0 REQUIRED) find_package(eigen3_cmake_module REQUIRED) find_package(OpenMP REQUIRED) # For parallel processing with Eigen From 93d43c4d9f54e6d51f88d718ef0fce43bd70a750 Mon Sep 17 00:00:00 2001 From: TristanWolfram Date: Tue, 26 Mar 2024 17:48:20 +0100 Subject: [PATCH 29/31] added documentation for PDAF and IPDA to README.md --- .../vortex_filtering/filters/README.md | 95 ++++++++++++++++++- .../include/vortex_filtering/filters/ipda.hpp | 29 +++--- 2 files changed, 109 insertions(+), 15 deletions(-) diff --git a/vortex-filtering/include/vortex_filtering/filters/README.md b/vortex-filtering/include/vortex_filtering/filters/README.md index b26afe81..dcd3f119 100644 --- a/vortex-filtering/include/vortex_filtering/filters/README.md +++ b/vortex-filtering/include/vortex_filtering/filters/README.md @@ -6,9 +6,11 @@ All classes and functions are under the namespace `vortex::filters`. Here is a great intro to Kalman filters in general: [Kalman and Bayesian Filters in Python](https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/00-Preface.ipynb) ## Overview -- [`ekf.hpp`](ekf.hpp) contains the EKF filter -- [`ukf.hpp`](ukf.hpp) contains the UKF filter +- [`ekf.hpp`](ekf.hpp) contains the EKF +- [`ukf.hpp`](ukf.hpp) contains the UKF - [`imm_filter.hpp`](imm_filter.hpp) contains the IMM filter +- [`pdaf.hpp`](pdaf.hpp) contains the pdaf +- [`ipda.hpp`](ipda.hpp) contains the ipda filter ### EKF This class represents an [Extended Kalman Filter](https://en.wikipedia.org/wiki/Extended_Kalman_filter). It is a template class with parameters `DynamicModelT` and `SensorModelT` for the dynamic model and sensor model respectively. It works with models derived from `vortex::models::DynamicModelLTV` and `vortex::models::SensorModelLTV`. All methods are static, so there is no need to create an instance of this class. @@ -226,4 +228,91 @@ Vec2d z_meas{48, 65}; using ImmFilter = vortex::filters::IMMFilter; auto [weights_upd, x_est_upds, x_est_preds, z_est_preds] = IMM::step(imm_model, sensor_model, dt, x_est_prevs, z_meas, model_weights, states_min_max); -``` \ No newline at end of file +``` + +### PDAF + +This class represents the **P**robalistic **D**ata **A**ssociation **F**ilter. It is implemented according to the textbook "Fundamentals of Sensor Fusion" (Chapter 7) by *Edmund Brekke*. The class works as a "toolbox" to provide the usage of the PDAF. Other classes can use the given functions to implement the PDAF for their particular tasks. + +However, the PDAF is mainly used by us for target tracking. Additionally, PDAF is **single**-target tracking. That means that all other measurements (besides the tracked target) are considered to be clutter. If multiple targets should be tracked, multiple single-target trackers can be initialized. Notable here is that the PDAF itself doesn't have to be initialized. In fact, it can't be initialized. It just provides static functions. Therefore, you can use the namespace and, with it, the PDAF functions. + +The main usage of the PDAF is the function **step()**. It will predict the next state of a given target. The step function will need several parameters to work: +* Dynamic model, +* sensor model, +* current time step, +* x estimation (the last known state of the target), +* z measurements, +* and config (which is a struct that holds:) + * mahalanobis threshold + * minimum gate threshold + * maximum gate threshold + * probability of detection + * clutter intensity + +#### Dynamic/Sensor Models + +If you want to use the PDAF for your measurements, you have to define how your measurements behave. That is what the models are for. The **dynamic** model (or transition model) describes how states change over time. A common example of this would be velocity. The **sensor** model is a bit less intuitive and difficult to explain. Let's consider the following example: +We want to measure the temperature inside a rocket drive. We know in theory how the temperature should change over time. That would be our dynamic model. We basically want to compare this model with the actual measurements and find a good estimated final value for the current state of the temperature in the drive. It's a bit of a design choice if you trust the model or the actual measurements. Both could be accurate or wrong. The main problem is that we can't put sensors directly into the drive (they would melt and be destroyed instantly). Therefore, we put a sensor outside the rocket and measure the temperature there. Here, we have to know how the temperature we measure corresponds to the actual temperature inside the drive. In other words, the temperature that we measure is not the actual value we need. But we know how to convert the measurement to the value we search for. This is the sensor model. If you want to get a better explanation of those models, I suggest using the Textbook "Artificial Intelligence: A Modern Approach, 4th Edition" by *Stuart Russell* and *Peter Norvig*. The book explains those models in Chapter 14 - *Probabilistic Reasoning Over Time*. All in all, you have to define those models and pass them to the function. Under the name space **vortex::models**, you can find simple predefined models to use: + +```c++ +// example how to define models using vortex::models +using DynMod = vortex::models::ConstantVelocity; +using SensorMod = vortex::models::IdentitySensorModel<4, 2>; +// example how to use PDAF in pratice +using PDAF = vortex::filter::PDAF; +auto [x_final, inside, outside, x_pred, z_pred, x_updated] = PDAF::step(parameters...) +``` + +#### Previous State Estimate +The step function needs to know what the previous state was. Based on this state, the dynamic model will be used. The model will give us a predicted new state. This state will be compared to the actual measurements. The previous state must be a Gaussian distribution. + +#### Measurements +The perceived measurements. This parameter consists of an Eigen vector. It should hold all perceived measurements. (The dimension must be the same as defined in the models.) + +#### Basic Principle +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 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. + +It is highly recommended to look into "Fundamentals of Sensor Fusion" (Chapter 7) by *Edmund Brekke*. You will find there a visualization of the explained process, which makes it easier to understand. Also, the implemented tests for the PDAF will plot the given outcome. This will also help to understand what is happening. +In order for the test to work with visualization you have to uncomment following statement in the CMakeLists.txt of *vortex-filtering*. +``` +if(BUILD_TESTING) + # add_compile_definitions(GNUPLOT_ENABLE=1) <- uncomment this + add_subdirectory(test) +endif() +``` + +#### Other Parameters + +* Current time step: The current iteration. +* Probability of detection: This is the probability that we will detect the target. Defining this, for example, with 0.8 means that there is a 20% chance that no measurement is assumed to be the target. (In this case, we will only take the dynamic model into consideration) +* Clutter intensity: Here you can define how much clutter you expect within the region. + +#### Returns + +The step()-function will return return the new estimated state and all outputs of the substeps. Returning those in addition, enables us to visualize the outputs of the PDAF. + +### IPDA + +IPDA stand for **I**ntegrated **P**robabilistic **D**ata **A**ssociation. It uses extra steps in addition to the regular PDAF. The PDAF is extended to also include a probability value which describes the target's existence. The main reason for this is to evaluate not only the state of the target but also the probability of existence dependent on the previous states. +IPDA works in terms of usage the same way the PDAF works. The class provides a static function (also called *step*). You can use this function by referencing the namespace. + +```c++ +using ConstantVelocity = vortex::models::ConstantVelocity; +using IdentitySensorModel = vortex::models::IdentitySensorModel<4, 2>; +using IPDA = vortex::filter::IPDA; + +auto [x_final, existence_pred, inside, outside, x_pred, z_pred, x_updated] = IPDA::step(parameters...); +``` + +#### Parameters +The parameters of the step() function are mainly the same as those of the PDAF. However, two parameters are added: +* Survival estimate: This is the survival estimate of the previous state estimate (corresponding to x_estimate). It describes the target existence, which we also want to estimate for the current state. +* Probability of survival: This is a general hyperparameter that defines how likely a target is to survive. This parameter shouldn't be confused with the probability of detection. If no measurement is considered to correspond to the target, we consider the dynamic model. The track still *survives*. If a target *dies*, it can't come back, and the track will be deleted. + +#### Returns +In addition to all the return parameters of the PDAF the IPDA will return the estimated existence prediction for the current state (corresponding to x_final). \ No newline at end of file diff --git a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp index 6e3a180b..e7d60f02 100644 --- a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp @@ -7,7 +7,9 @@ namespace vortex::filter { -template class IPDA { +template +class IPDA +{ public: static constexpr int N_DIM_x = DynModT::N_DIM_x; static constexpr int N_DIM_z = SensModT::N_DIM_z; @@ -19,11 +21,11 @@ template ; - using Gauss_z = typename T::Gauss_z; - using Gauss_x = typename T::Gauss_x; - using Vec_z = typename T::Vec_z; + using Gauss_z = typename T::Gauss_z; + using Gauss_x = typename T::Gauss_x; + using Vec_z = typename T::Vec_z; using GaussMix = vortex::prob::GaussianMixture; - using PDAF = vortex::filter::PDAF>; + using PDAF = vortex::filter::PDAF>; IPDA() = delete; @@ -32,13 +34,16 @@ template & measurements, double probability_of_survival, double last_detection_probability_, double probability_of_detection, double clutter_intensity, Gauss_z& z_pred) From b98ee43f6c0883bc85b8d1baf478534a73560881 Mon Sep 17 00:00:00 2001 From: TristanWolfram Date: Tue, 7 May 2024 15:29:24 +0200 Subject: [PATCH 30/31] added docstring and minor changes --- .../vortex_filtering/filters/README.md | 2 +- .../include/vortex_filtering/filters/ipda.hpp | 30 ++- .../include/vortex_filtering/filters/pdaf.hpp | 144 +++++++++++---- vortex-filtering/test/pdaf_test.cpp | 173 +++++++++++------- 4 files changed, 247 insertions(+), 102 deletions(-) diff --git a/vortex-filtering/include/vortex_filtering/filters/README.md b/vortex-filtering/include/vortex_filtering/filters/README.md index dcd3f119..7122ea53 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 shoul 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. 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 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/ipda.hpp b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp index e7d60f02..a046ffe1 100644 --- a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp @@ -1,3 +1,13 @@ +/** + * @file ipda.hpp + * @author Tristan Wolfram + * @brief File for the IPDA filter + * @version 1.0 + * @date 2024-05-07 + * + * @copyright Copyright (c) 2024 + * + */ #pragma once #include #include @@ -7,6 +17,11 @@ namespace vortex::filter { +/** + * @brief The IPDA filter class + * @tparam DynModT The dynamic model type + * @tparam SensModT The sensor model type + */ template class IPDA { @@ -25,7 +40,7 @@ class IPDA using Gauss_x = typename T::Gauss_x; using Vec_z = typename T::Vec_z; using GaussMix = vortex::prob::GaussianMixture; - using PDAF = vortex::filter::PDAF>; + using PDAF = vortex::filter::PDAF; IPDA() = delete; @@ -60,6 +75,19 @@ class IPDA return (l_k * predicted_existence_probability) / (1 - (1 - l_k) * predicted_existence_probability); } + /** + * @brief The IPDAF step function. Gets following parameters and calculates the next state with the probablity of + * existence. + * @param dyn_model The dynamic model. + * @param sen_model The sensor model. + * @param timestep The timestep. + * @param x_est The estimated state. + * @param z_meas The percieved measurements. + * @param survive_est The estimated survival probability (current state). + * @param config configuration data - see Config struct of PDAF. + * @return A tuple containing the final state, the existence probability, the inside (of the gate) measurements, the + * outside (of the gate) measurements, the predicted state, the predicted measurements, and the updated state. + */ static std::tuple, std::vector, Gauss_x, Gauss_z, std::vector> step(const DynModT& dyn_model, const SensModT& sen_model, double timestep, const Gauss_x& x_est, const std::vector& z_meas, double survive_est, const IPDA::Config& config) diff --git a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp index 5d838134..861ce899 100644 --- a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp @@ -1,3 +1,13 @@ +/** + * @file ipda.hpp + * @author Tristan Wolfram + * @brief File for the PDAF filter + * @version 1.0 + * @date 2024-05-07 + * + * @copyright Copyright (c) 2024 + * + */ #pragma once #include @@ -7,9 +17,16 @@ #include #include -namespace vortex::filter { - -template class PDAF { +namespace vortex::filter +{ +/** + * @brief The PDAF filter class + * @tparam DynModT The dynamic model + * @tparam SensModT The sensor model + */ +template +class PDAF +{ public: static constexpr int N_DIM_x = DynModT::N_DIM_x; static constexpr int N_DIM_z = SensModT::N_DIM_z; @@ -19,62 +36,105 @@ template ; - using EKF = vortex::filter::EKF_t; - using Gauss_z = typename T::Gauss_z; - using Gauss_x = typename T::Gauss_x; - using Vec_z = typename T::Vec_z; + using EKF = vortex::filter::EKF_t; + using Gauss_z = typename T::Gauss_z; + using Gauss_x = typename T::Gauss_x; + using Vec_z = typename T::Vec_z; using MeasurementsZd = std::vector; - using StatesXd = std::vector; - using GaussMixZd = vortex::prob::GaussianMixture; + using StatesXd = std::vector; + using GaussMixXd = vortex::prob::GaussianMixture; - struct Config { + struct Config + { double mahalanobis_threshold = 1.0; - double min_gate_threshold = 0.0; - double max_gate_threshold = HUGE_VAL; - double prob_of_detection = 1.0; - double clutter_intensity = 1.0; + double min_gate_threshold = 0.0; + double max_gate_threshold = HUGE_VAL; + double prob_of_detection = 1.0; + double clutter_intensity = 1.0; }; PDAF() = delete; + /** + * @brief The step function for the PDAF filter + * @param dyn_model The dynamic model + * @param sen_model The sensor model + * @param timestep The timestep + * @param x_est The estimated state (current state) + * @param z_meas The measurements + * @param config The configuration + * @return A tuple containing the (new) estimated state, the measurements that were inside the gate, the measurements + * that were outside the gate, the predicted state (from dynamic model), the predicted measurement (from sensor + * model), and the updated states (comparisson of prediced state and actual measurements) + */ static std::tuple - step(const DynModT &dyn_model, const SensModT &sen_model, double timestep, const Gauss_x &x_est, const MeasurementsZd &z_meas, const Config &config) + step(const DynModT& dyn_model, const SensModT& sen_model, double timestep, const Gauss_x& x_est, + const MeasurementsZd& z_meas, const Config& config) { - auto [x_pred, z_pred] = EKF::predict(dyn_model, sen_model, timestep, x_est); - auto [inside, outside] = apply_gate(z_meas, z_pred, config.mahalanobis_threshold, config.min_gate_threshold, config.max_gate_threshold); + auto [x_pred, z_pred] = EKF::predict(dyn_model, sen_model, timestep, x_est); + auto [inside, outside] = + apply_gate(z_meas, z_pred, config.mahalanobis_threshold, config.min_gate_threshold, config.max_gate_threshold); StatesXd x_updated; - for (const auto &measurement : inside) { + for (const auto& measurement : inside) + { x_updated.push_back(EKF::update(sen_model, x_pred, z_pred, measurement)); } - Gauss_x x_final = get_weighted_average(inside, x_updated, z_pred, x_pred, config.prob_of_detection, config.clutter_intensity); - return {x_final, inside, outside, x_pred, z_pred, x_updated}; + Gauss_x x_final = + get_weighted_average(inside, x_updated, z_pred, x_pred, config.prob_of_detection, config.clutter_intensity); + return { x_final, inside, outside, x_pred, z_pred, x_updated }; } - static std::tuple apply_gate(const MeasurementsZd &z_meas, const Gauss_z &z_pred, double mahalanobis_threshold, - double min_gate_threshold = 0.0, double max_gate_threshold = HUGE_VAL) + /** + * @brief Applies the gate to the measurements + * @param z_meas The measurements + * @param z_pred The predicted measurement + * @param mahalanobis_threshold The threshold for the Mahalanobis distance + * @param min_gate_threshold The minimum threshold for the gate + * @param max_gate_threshold The maximum threshold for the gate + * @return A tuple containing the measurements that were inside the gate and the measurements that were outside the + * gate + */ + static std::tuple apply_gate(const MeasurementsZd& z_meas, const Gauss_z& z_pred, + double mahalanobis_threshold, + double min_gate_threshold = 0.0, + double max_gate_threshold = HUGE_VAL) { MeasurementsZd inside_meas; MeasurementsZd outside_meas; - for (const auto &measurement : z_meas) { + for (const auto& measurement : z_meas) + { double mahalanobis_distance = z_pred.mahalanobis_distance(measurement); - double regular_distance = (z_pred.mean() - measurement).norm(); - if ((mahalanobis_distance <= mahalanobis_threshold || regular_distance <= min_gate_threshold) && regular_distance <= max_gate_threshold) { + double regular_distance = (z_pred.mean() - measurement).norm(); + if ((mahalanobis_distance <= mahalanobis_threshold || regular_distance <= min_gate_threshold) && + regular_distance <= max_gate_threshold) + { inside_meas.push_back(measurement); } - else { + else + { outside_meas.push_back(measurement); } } - return {inside_meas, outside_meas}; + return { inside_meas, outside_meas }; } - // Getting weighted average of the predicted states - static Gauss_x get_weighted_average(const MeasurementsZd &z_meas, const StatesXd &updated_states, const Gauss_z &z_pred, const Gauss_x &x_pred, - double prob_of_detection, double clutter_intensity) + /** + * @brief Calculates the weighted average of the measurements + * @param z_meas The measurements + * @param updated_states The updated states + * @param z_pred The predicted measurement + * @param x_pred The predicted state + * @param prob_of_detection The probability of detection + * @param clutter_intensity The clutter intensity + * @return The weighted average of the measurements + */ + static Gauss_x get_weighted_average(const MeasurementsZd& z_meas, const StatesXd& updated_states, + const Gauss_z& z_pred, const Gauss_x& x_pred, double prob_of_detection, + double clutter_intensity) { StatesXd states; states.push_back(x_pred); @@ -82,22 +142,34 @@ template see association probabilities in textbook p. 123 + * "Corollary 7.3.3 + * @param z_meas The measurements + * @param z_pred The predicted measurement + * @param prob_of_detection The probability of detection + * @param clutter_intensity The clutter intensity + * @return The weights for the measurements -> same order as in z_meas + * The first element is the weight for no association + * length of weights = z_meas.size() + 1 + */ + static Eigen::VectorXd get_weights(const MeasurementsZd& z_meas, const Gauss_z& z_pred, double prob_of_detection, + double clutter_intensity) { Eigen::VectorXd weights(z_meas.size() + 1); // in case no measurement assosiates with the target double no_association = clutter_intensity * (1 - prob_of_detection); - weights(0) = no_association; + weights(0) = no_association; // measurements associating with the target - for (size_t k = 1; k < z_meas.size() + 1; k++) { + for (size_t k = 1; k < z_meas.size() + 1; k++) + { weights(k) = (prob_of_detection * z_pred.pdf(z_meas.at(k - 1))); } @@ -108,4 +180,4 @@ template #include -using ConstantVelocity = vortex::models::ConstantVelocity; +using ConstantVelocity = vortex::models::ConstantVelocity; using IdentitySensorModel = vortex::models::IdentitySensorModel<4, 2>; -using PDAF = vortex::filter::PDAF; +using PDAF = vortex::filter::PDAF; // testing the get_weights function TEST(PDAF, get_weights_is_calculating) @@ -174,7 +174,7 @@ TEST(PDAF, apply_gate_is_separating_correctly) EXPECT_EQ(inside[0], meas[0]); EXPECT_EQ(outside[0], meas[1]); - #if (GNUPLOT_ENABLE) +#if (GNUPLOT_ENABLE) Gnuplot gp; gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; gp << "set size ratio -1\n"; @@ -190,10 +190,11 @@ TEST(PDAF, apply_gate_is_separating_correctly) vortex::utils::Ellipse prediction = vortex::plotting::gauss_to_ellipse(z_pred, mahalanobis_threshold); - gp << "set object " << ++object_counter << " ellipse center " << prediction.x() << "," << prediction.y() << " size " << prediction.major_axis() << "," - << prediction.minor_axis() << " angle " << prediction.angle_deg() << "fs empty border lc rgb 'cyan'\n"; + gp << "set object " << ++object_counter << " ellipse center " << prediction.x() << "," << prediction.y() << " size " + << prediction.major_axis() << "," << prediction.minor_axis() << " angle " << prediction.angle_deg() + << "fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; - #endif +#endif } TEST(PDAF, apply_gate_is_separating_correctly_2) @@ -208,7 +209,7 @@ TEST(PDAF, apply_gate_is_separating_correctly_2) EXPECT_EQ(inside.size(), 5u); EXPECT_EQ(outside.size(), 1u); - #if (GNUPLOT_ENABLE) +#if (GNUPLOT_ENABLE) Gnuplot gp; gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; gp << "set size ratio -1\n"; @@ -224,10 +225,11 @@ TEST(PDAF, apply_gate_is_separating_correctly_2) vortex::utils::Ellipse prediction = vortex::plotting::gauss_to_ellipse(z_pred, mahalanobis_threshold); - gp << "set object " << ++object_counter << " ellipse center " << prediction.x() << "," << prediction.y() << " size " << prediction.major_axis() << "," - << prediction.minor_axis() << " angle " << prediction.angle_deg() << "fs empty border lc rgb 'cyan'\n"; + gp << "set object " << ++object_counter << " ellipse center " << prediction.x() << "," << prediction.y() << " size " + << prediction.major_axis() << "," << prediction.minor_axis() << " angle " << prediction.angle_deg() + << "fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; - #endif +#endif } // testing the predict_next_state function @@ -241,14 +243,14 @@ TEST(PDAF, predict_next_state_is_calculating) std::vector meas = { { 0.0, 1.0 }, { 1.0, 0.0 }, { 1.0, 1.0 }, { 0.0, 2.0 }, { 2.0, 0.0 }, { 2.0, 2.0 } }; - ConstantVelocity dyn_model{1.0}; - IdentitySensorModel sen_model{1.0}; + ConstantVelocity dyn_model{ 1.0 }; + IdentitySensorModel sen_model{ 1.0 }; auto [x_final, inside, outside, x_pred, z_pred, x_updated] = PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); std::cout << "x_final: " << x_final.mean() << std::endl; - #if (GNUPLOT_ENABLE) +#if (GNUPLOT_ENABLE) Gnuplot gp; gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; gp << "set size ratio -1\n"; @@ -264,10 +266,12 @@ TEST(PDAF, predict_next_state_is_calculating) vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2, 2)); vortex::utils::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); - gp << "set object " << ++object_counter << " ellipse center " << ellipse.x() << "," << ellipse.y() << " size " << ellipse.major_axis() << "," - << ellipse.minor_axis() << " angle " << ellipse.angle_deg() << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + gp << "set object " << ++object_counter << " ellipse center " << ellipse.x() << "," << ellipse.y() << " size " + << ellipse.major_axis() << "," << ellipse.minor_axis() << " angle " << ellipse.angle_deg() + << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; - gp << "set object " << ++object_counter << " circle center " << ellipse.x() << "," << ellipse.y() << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; + gp << "set object " << ++object_counter << " circle center " << ellipse.x() << "," << ellipse.y() << " size " + << 0.02 << " fs empty border lc rgb 'blue'\n"; } gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " @@ -283,11 +287,12 @@ TEST(PDAF, predict_next_state_is_calculating) << x_final.mean()(1) << " nohead lc rgb 'green'\n"; vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.mahalanobis_threshold); - gp << "set object " << ++object_counter << " ellipse center " << gate.x() << "," << gate.y() << " size " << gate.major_axis() << "," << gate.minor_axis() - << " angle " << gate.angle_deg() << " fs empty border lc rgb 'cyan'\n"; + gp << "set object " << ++object_counter << " ellipse center " << gate.x() << "," << gate.y() << " size " + << gate.major_axis() << "," << gate.minor_axis() << " angle " << gate.angle_deg() + << " fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; - #endif +#endif } TEST(PDAF, predict_next_state_2) @@ -300,14 +305,14 @@ TEST(PDAF, predict_next_state_2) std::vector meas = { { -3.0, -3.0 }, { 0.0, 0.0 }, { -1.2, 1.5 }, { -2.0, -2.0 }, { 2.0, 0.0 }, { -1.0, 1.0 } }; - ConstantVelocity dyn_model{1.0}; - IdentitySensorModel sen_model{0.5}; + ConstantVelocity dyn_model{ 1.0 }; + IdentitySensorModel sen_model{ 0.5 }; auto [x_final, inside, outside, x_pred, z_pred, x_updated] = PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); std::cout << "x_final: " << x_final.mean() << std::endl; - #if (GNUPLOT_ENABLE) +#if (GNUPLOT_ENABLE) Gnuplot gp; gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; gp << "set size ratio -1\n"; @@ -323,10 +328,12 @@ TEST(PDAF, predict_next_state_2) vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2, 2)); vortex::utils::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); - gp << "set object " << ++object_counter << " ellipse center " << ellipse.x() << "," << ellipse.y() << " size " << ellipse.major_axis() << "," - << ellipse.minor_axis() << " angle " << ellipse.angle_deg() << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + gp << "set object " << ++object_counter << " ellipse center " << ellipse.x() << "," << ellipse.y() << " size " + << ellipse.major_axis() << "," << ellipse.minor_axis() << " angle " << ellipse.angle_deg() + << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; - gp << "set object " << ++object_counter << " circle center " << ellipse.x() << "," << ellipse.y() << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; + gp << "set object " << ++object_counter << " circle center " << ellipse.x() << "," << ellipse.y() << " size " + << 0.02 << " fs empty border lc rgb 'blue'\n"; } gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " @@ -342,11 +349,12 @@ TEST(PDAF, predict_next_state_2) << x_final.mean()(1) << " nohead lc rgb 'green'\n"; vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.mahalanobis_threshold); - gp << "set object " << ++object_counter << " ellipse center " << gate.x() << "," << gate.y() << " size " << gate.major_axis() << "," << gate.minor_axis() - << " angle " << gate.angle_deg() << " fs empty border lc rgb 'cyan'\n"; + gp << "set object " << ++object_counter << " ellipse center " << gate.x() << "," << gate.y() << " size " + << gate.major_axis() << "," << gate.minor_axis() << " angle " << gate.angle_deg() + << " fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; - #endif +#endif } TEST(PDAF, predict_next_state_3_1) @@ -359,14 +367,14 @@ TEST(PDAF, predict_next_state_3_1) std::vector meas = { { 0.0, 0.5 }, { 0.2, 0.2 }, { 0.8, 2.3 }, { 0.5, 0.0 }, { 4.2, 2.7 }, { 1.4, 2.5 } }; - ConstantVelocity dyn_model{0.5}; - IdentitySensorModel sen_model{1.0}; + ConstantVelocity dyn_model{ 0.5 }; + IdentitySensorModel sen_model{ 1.0 }; auto [x_final, inside, outside, x_pred, z_pred, x_updated] = PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); std::cout << "x_final: " << x_final.mean() << std::endl; - #if (GNUPLOT_ENABLE) +#if (GNUPLOT_ENABLE) Gnuplot gp; gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; gp << "set size ratio -1\n"; @@ -382,10 +390,12 @@ TEST(PDAF, predict_next_state_3_1) vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2, 2)); vortex::utils::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); - gp << "set object " << ++object_counter << " ellipse center " << ellipse.x() << "," << ellipse.y() << " size " << ellipse.major_axis() << "," - << ellipse.minor_axis() << " angle " << ellipse.angle_deg() << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + gp << "set object " << ++object_counter << " ellipse center " << ellipse.x() << "," << ellipse.y() << " size " + << ellipse.major_axis() << "," << ellipse.minor_axis() << " angle " << ellipse.angle_deg() + << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; - gp << "set object " << ++object_counter << " circle center " << ellipse.x() << "," << ellipse.y() << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; + gp << "set object " << ++object_counter << " circle center " << ellipse.x() << "," << ellipse.y() << " size " + << 0.02 << " fs empty border lc rgb 'blue'\n"; } gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " @@ -403,11 +413,12 @@ TEST(PDAF, predict_next_state_3_1) << x_final.mean()(1) << " nohead lc rgb 'green'\n"; vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.mahalanobis_threshold); - gp << "set object " << ++object_counter << " ellipse center " << gate.x() << "," << gate.y() << " size " << gate.major_axis() << "," << gate.minor_axis() - << " angle " << gate.angle_deg() << " fs empty border lc rgb 'cyan'\n"; + gp << "set object " << ++object_counter << " ellipse center " << gate.x() << "," << gate.y() << " size " + << gate.major_axis() << "," << gate.minor_axis() << " angle " << gate.angle_deg() + << " fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; - #endif +#endif } TEST(PDAF, predict_next_state_3_2) @@ -421,14 +432,14 @@ TEST(PDAF, predict_next_state_3_2) std::vector meas = { { -0.5, 2.0 }, { -0.23, 0.5 }, { -2.0, 3.4 }, { 0.0, 1.3 }, { 0.14, 0.5 }, { -2.5, 0.89 } }; - ConstantVelocity dyn_model{0.5}; - IdentitySensorModel sen_model{1.0}; + ConstantVelocity dyn_model{ 0.5 }; + IdentitySensorModel sen_model{ 1.0 }; auto [x_final, inside, outside, x_pred, z_pred, x_updated] = PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); std::cout << "x_final: " << x_final.mean() << std::endl; - #if (GNUPLOT_ENABLE) +#if (GNUPLOT_ENABLE) Gnuplot gp; gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; gp << "set size ratio -1\n"; @@ -444,10 +455,12 @@ TEST(PDAF, predict_next_state_3_2) vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2, 2)); vortex::utils::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); - gp << "set object " << ++object_counter << " ellipse center " << ellipse.x() << "," << ellipse.y() << " size " << ellipse.major_axis() << "," - << ellipse.minor_axis() << " angle " << ellipse.angle_deg() << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + gp << "set object " << ++object_counter << " ellipse center " << ellipse.x() << "," << ellipse.y() << " size " + << ellipse.major_axis() << "," << ellipse.minor_axis() << " angle " << ellipse.angle_deg() + << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; - gp << "set object " << ++object_counter << " circle center " << ellipse.x() << "," << ellipse.y() << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; + gp << "set object " << ++object_counter << " circle center " << ellipse.x() << "," << ellipse.y() << " size " + << 0.02 << " fs empty border lc rgb 'blue'\n"; } gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " @@ -469,11 +482,12 @@ TEST(PDAF, predict_next_state_3_2) << " nohead lc rgb 'orange-red'\n"; vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.mahalanobis_threshold); - gp << "set object " << ++object_counter << " ellipse center " << gate.x() << "," << gate.y() << " size " << gate.major_axis() << "," << gate.minor_axis() - << " angle " << gate.angle_deg() << " fs empty border lc rgb 'cyan'\n"; + gp << "set object " << ++object_counter << " ellipse center " << gate.x() << "," << gate.y() << " size " + << gate.major_axis() << "," << gate.minor_axis() << " angle " << gate.angle_deg() + << " fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; - #endif +#endif } TEST(PDAF, predict_next_state_3_3) @@ -486,14 +500,14 @@ TEST(PDAF, predict_next_state_3_3) std::vector meas = { { -1.5, 2.5 }, { -1.2, 2.7 }, { -0.8, 2.3 }, { -1.7, 1.9 }, { -2.0, 3.0 }, { -1.11, 2.04 } }; - ConstantVelocity dyn_model{0.5}; - IdentitySensorModel sen_model{1.0}; + ConstantVelocity dyn_model{ 0.5 }; + IdentitySensorModel sen_model{ 1.0 }; auto [x_final, inside, outside, x_pred, z_pred, x_updated] = PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); std::cout << "x_final: " << x_final.mean() << std::endl; - #if (GNUPLOT_ENABLE) +#if (GNUPLOT_ENABLE) Gnuplot gp; gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; gp << "set size ratio -1\n"; @@ -509,10 +523,12 @@ TEST(PDAF, predict_next_state_3_3) vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2, 2)); vortex::utils::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); - gp << "set object " << ++object_counter << " ellipse center " << ellipse.x() << "," << ellipse.y() << " size " << ellipse.major_axis() << "," - << ellipse.minor_axis() << " angle " << ellipse.angle_deg() << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + gp << "set object " << ++object_counter << " ellipse center " << ellipse.x() << "," << ellipse.y() << " size " + << ellipse.major_axis() << "," << ellipse.minor_axis() << " angle " << ellipse.angle_deg() + << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; - gp << "set object " << ++object_counter << " circle center " << ellipse.x() << "," << ellipse.y() << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; + gp << "set object " << ++object_counter << " circle center " << ellipse.x() << "," << ellipse.y() << " size " + << 0.02 << " fs empty border lc rgb 'blue'\n"; } gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " @@ -538,11 +554,12 @@ TEST(PDAF, predict_next_state_3_3) << " nohead lc rgb 'orange-red'\n"; vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.mahalanobis_threshold); - gp << "set object " << ++object_counter << " ellipse center " << gate.x() << "," << gate.y() << " size " << gate.major_axis() << "," << gate.minor_axis() - << " angle " << gate.angle_deg() << " fs empty border lc rgb 'cyan'\n"; + gp << "set object " << ++object_counter << " ellipse center " << gate.x() << "," << gate.y() << " size " + << gate.major_axis() << "," << gate.minor_axis() << " angle " << gate.angle_deg() + << " fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; - #endif +#endif } TEST(PDAF, predict_next_state_3_4) @@ -555,14 +572,14 @@ TEST(PDAF, predict_next_state_3_4) std::vector meas = { { -2.0, 2.2 }, { -1.8, 2.3 }, { -2.3, 2.0 }, { 0.6, 1.5 }, { -2.0, 2.0 }, { -1.4, 2.5 } }; - ConstantVelocity dyn_model{0.5}; - IdentitySensorModel sen_model{1.0}; + ConstantVelocity dyn_model{ 0.5 }; + IdentitySensorModel sen_model{ 1.0 }; auto [x_final, inside, outside, x_pred, z_pred, x_updated] = PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); std::cout << "x_final: " << x_final.mean() << std::endl; - #if (GNUPLOT_ENABLE) +#if (GNUPLOT_ENABLE) Gnuplot gp; gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; gp << "set size ratio -1\n"; @@ -578,10 +595,12 @@ TEST(PDAF, predict_next_state_3_4) vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2, 2)); vortex::utils::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); - gp << "set object " << ++object_counter << " ellipse center " << ellipse.x() << "," << ellipse.y() << " size " << ellipse.major_axis() << "," - << ellipse.minor_axis() << " angle " << ellipse.angle_deg() << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + gp << "set object " << ++object_counter << " ellipse center " << ellipse.x() << "," << ellipse.y() << " size " + << ellipse.major_axis() << "," << ellipse.minor_axis() << " angle " << ellipse.angle_deg() + << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; - gp << "set object " << ++object_counter << " circle center " << ellipse.x() << "," << ellipse.y() << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; + gp << "set object " << ++object_counter << " circle center " << ellipse.x() << "," << ellipse.y() << " size " + << 0.02 << " fs empty border lc rgb 'blue'\n"; } gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " @@ -611,9 +630,35 @@ TEST(PDAF, predict_next_state_3_4) << " nohead lc rgb 'orange-red'\n"; vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.mahalanobis_threshold); - gp << "set object " << ++object_counter << " ellipse center " << gate.x() << "," << gate.y() << " size " << gate.major_axis() << "," << gate.minor_axis() - << " angle " << gate.angle_deg() << " fs empty border lc rgb 'cyan'\n"; + gp << "set object " << ++object_counter << " ellipse center " << gate.x() << "," << gate.y() << " size " + << gate.major_axis() << "," << gate.minor_axis() << " angle " << gate.angle_deg() + << " fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; - #endif +#endif +} + +TEST(PDAF_2, test_with_other_model) +{ + using TestDynamicModel = vortex::models::ConstantAcceleration; + using TestSensorModel = vortex::models::IdentitySensorModel<6, 2>; + using PDAF = vortex::filter::PDAF; + + PDAF::Config config; + config.mahalanobis_threshold = 1.0; + config.prob_of_detection = 0.8; + config.clutter_intensity = 1.0; + vortex::prob::Gauss6d x_est({ 0.0, 1.0, 3.0, -1.0, 0.0, 0.5 }, Eigen::MatrixXd::Identity(6, 6)); + std::vector meas = { { 0.0, 1.0 }, { 1.0, 0.0 }, { 1.0, 1.0 }, + { 0.0, 2.0 }, { 2.0, 0.0 }, { 2.0, 2.0 } }; + + TestDynamicModel dyn_model{ 1.0, 1.0 }; + TestSensorModel sen_model{ 1.0 }; + + auto [x_final, inside, outside, x_pred, z_pred, x_updated] = + PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); + std::cout << "x_final: " << x_final.mean() << std::endl; + + // it compiles -> other template parameters are working with the PDAF class + ASSERT_TRUE(true); } \ No newline at end of file From b7b7ed01728f1fb6cef700101844dbf162ff753d Mon Sep 17 00:00:00 2001 From: TristanWolfram Date: Tue, 7 May 2024 16:14:43 +0200 Subject: [PATCH 31/31] updated readme --- vortex-filtering/include/vortex_filtering/filters/README.md | 6 +++--- vortex-filtering/include/vortex_filtering/filters/ipda.hpp | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/vortex-filtering/include/vortex_filtering/filters/README.md b/vortex-filtering/include/vortex_filtering/filters/README.md index 7122ea53..28eeec68 100644 --- a/vortex-filtering/include/vortex_filtering/filters/README.md +++ b/vortex-filtering/include/vortex_filtering/filters/README.md @@ -252,7 +252,7 @@ The main usage of the PDAF is the function **step()**. It will predict the next #### Dynamic/Sensor Models If you want to use the PDAF for your measurements, you have to define how your measurements behave. That is what the models are for. The **dynamic** model (or transition model) describes how states change over time. A common example of this would be velocity. The **sensor** model is a bit less intuitive and difficult to explain. Let's consider the following example: -We want to measure the temperature inside a rocket drive. We know in theory how the temperature should change over time. That would be our dynamic model. We basically want to compare this model with the actual measurements and find a good estimated final value for the current state of the temperature in the drive. It's a bit of a design choice if you trust the model or the actual measurements. Both could be accurate or wrong. The main problem is that we can't put sensors directly into the drive (they would melt and be destroyed instantly). Therefore, we put a sensor outside the rocket and measure the temperature there. Here, we have to know how the temperature we measure corresponds to the actual temperature inside the drive. In other words, the temperature that we measure is not the actual value we need. But we know how to convert the measurement to the value we search for. This is the sensor model. If you want to get a better explanation of those models, I suggest using the Textbook "Artificial Intelligence: A Modern Approach, 4th Edition" by *Stuart Russell* and *Peter Norvig*. The book explains those models in Chapter 14 - *Probabilistic Reasoning Over Time*. All in all, you have to define those models and pass them to the function. Under the name space **vortex::models**, you can find simple predefined models to use: +We want to measure the temperature inside a rocket drive. We know in theory, how the temperature should change over time. That would be our dynamic model. We basically want to compare this model with the actual measurements and find a good estimated final value for the current state of the temperature in the drive. It's a bit of a design choice if you trust the model or the actual measurements. Both could be accurate or wrong. The main problem is that we can't put sensors directly into the drive (they would melt and be destroyed instantly). Therefore, we put a sensor outside the rocket and measure the temperature there. Here, we have to know how the temperature we measure corresponds to the actual temperature inside the drive. In other words, the temperature that we measure is not the actual value of what we want to measure. But we know which value corresponds to the measurent value (a measured value of X degrees outside the drive corresponds to Y degrees inside the drive). This is given by the sensor model. If you want to get a better explanation of those models, I suggest using the Textbook "Artificial Intelligence: A Modern Approach, 4th Edition" by *Stuart Russell* and *Peter Norvig*. The book explains those models in Chapter 14 - *Probabilistic Reasoning Over Time*. All in all, you have to define those models and pass them to the function. Under the name space **vortex::models**, you can find simple predefined models to use: ```c++ // example how to define models using vortex::models @@ -267,7 +267,7 @@ auto [x_final, inside, outside, x_pred, z_pred, x_updated] = PDAF::step(paramete The step function needs to know what the previous state was. Based on this state, the dynamic model will be used. The model will give us a predicted new state. This state will be compared to the actual measurements. The previous state must be a Gaussian distribution. #### Measurements -The perceived measurements. This parameter consists of an Eigen vector. It should hold all perceived measurements. (The dimension must be the same as defined in the models.) +The perceived measurements. This parameter consists of an `Eigen::Vector`. It should hold all perceived measurements. (The dimension must be the same as defined in the models.) #### Basic Principle 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. @@ -311,7 +311,7 @@ auto [x_final, existence_pred, inside, outside, x_pred, z_pred, x_updated] = IPD #### Parameters The parameters of the step() function are mainly the same as those of the PDAF. However, two parameters are added: -* Survival estimate: This is the survival estimate of the previous state estimate (corresponding to x_estimate). It describes the target existence, which we also want to estimate for the current state. +* Existence Probability: This is the survival estimate of the previous state estimate (corresponding to x_estimate). It describes the target existence, which we also want to estimate for the current state. * Probability of survival: This is a general hyperparameter that defines how likely a target is to survive. This parameter shouldn't be confused with the probability of detection. If no measurement is considered to correspond to the target, we consider the dynamic model. The track still *survives*. If a target *dies*, it can't come back, and the track will be deleted. #### Returns diff --git a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp index a046ffe1..ab500088 100644 --- a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp @@ -83,20 +83,20 @@ class IPDA * @param timestep The timestep. * @param x_est The estimated state. * @param z_meas The percieved measurements. - * @param survive_est The estimated survival probability (current state). + * @param existence_prob The estimated survival probability (current state). * @param config configuration data - see Config struct of PDAF. * @return A tuple containing the final state, the existence probability, the inside (of the gate) measurements, the * outside (of the gate) measurements, the predicted state, the predicted measurements, and the updated state. */ static std::tuple, std::vector, Gauss_x, Gauss_z, std::vector> step(const DynModT& dyn_model, const SensModT& sen_model, double timestep, const Gauss_x& x_est, - const std::vector& z_meas, double survive_est, const IPDA::Config& config) + const std::vector& z_meas, double existence_prob, const IPDA::Config& config) { auto [x_final, inside, outside, x_pred, z_pred, x_updated] = PDAF::step(dyn_model, sen_model, timestep, x_est, z_meas, static_cast(config)); double existence_probability = get_existence_probability( - inside, config.prob_of_survival, survive_est, config.prob_of_detection, config.clutter_intensity, z_pred); + inside, config.prob_of_survival, existence_prob, config.prob_of_detection, config.clutter_intensity, z_pred); return { x_final, existence_probability, inside, outside, x_pred, z_pred, x_updated }; } };