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