From 71655d801a5f2974e97560176e47628f7a58f3d7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Eirik=20Kol=C3=A5s?= Date: Thu, 4 Jul 2024 22:38:51 +0200 Subject: [PATCH] fixed tests. Missing documentation --- .../include/vortex_filtering/filters/ipda.hpp | 52 +---- .../include/vortex_filtering/filters/pdaf.hpp | 137 +++-------- vortex-filtering/test/pdaf_test.cpp | 216 +++++++++--------- 3 files changed, 143 insertions(+), 262 deletions(-) diff --git a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp index e7bae1a8..52eb2430 100644 --- a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp @@ -1,22 +1,11 @@ -/** - * @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 +#include #include #include #include -namespace vortex::filter -{ +namespace vortex::filter { namespace config { struct IPDA { @@ -26,14 +15,7 @@ struct IPDA { }; } // namespace config -/** - * @brief The IPDA filter class - * @tparam DynModT The dynamic model type - * @tparam SensModT The sensor model type - */ -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; @@ -43,10 +25,10 @@ class IPDA using T = Types_xzuvw; - using Arr_zXd = Eigen::Array; - using Arr_1Xb = Eigen::Array; - using EKF = vortex::filter::EKF_t; - using PDAF = vortex::filter::PDAF; + using Arr_zXd = Eigen::Array; + using Arr_1Xb = Eigen::Array; + using EKF = vortex::filter::EKF_t; + using PDAF = vortex::filter::PDAF; IPDA() = delete; @@ -143,22 +125,8 @@ class IPDA return 1 / V_k * (m_k - r_k * P_d); // (7.31) } - /** - * @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 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_mod, const SensModT &sens_mod, double timestep, const Gauss_x &x_est, const std::vector &z_meas, double existence_prob_est, - IPDA::Config &config) + static Output step(const DynModT &dyn_mod, const SensModT &sens_mod, double timestep, const State &state_est_prev, const Arr_zXd &z_measurements, + Config &config) { double existence_prob_pred = existence_prediction(state_est_prev.existence_probability, config.ipda.prob_of_survival); @@ -191,4 +159,4 @@ class IPDA // clang-format on } }; -} // namespace vortex::filter +} // namespace vortex::filter \ No newline at end of file diff --git a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp index 26b03b2c..95e72266 100644 --- a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp @@ -1,13 +1,3 @@ -/** - * @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 @@ -19,6 +9,7 @@ #include #include +namespace vortex::filter { namespace config { struct PDAF { @@ -30,16 +21,6 @@ struct PDAF { }; } // namespace config -namespace vortex::filter -{ -/** - * @brief The PDAF filter class - * @tparam DynModT The dynamic model - * @tparam SensModT The sensor model - */ -template -class PDAF -{ template class PDAF { public: static constexpr int N_DIM_x = DynModT::N_DIM_x; @@ -54,27 +35,14 @@ template ; using Arr_1Xb = Eigen::Array; using Gauss_xX = std::vector; using EKF = vortex::filter::EKF_t; - 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; - }; - - struct Output { - Gauss_x x_; - Gauss_x x_prediction; - Gauss_z z_prediction; - Gauss_xX x_updates; - Arr_1Xb gated_measurements; + struct Config { + config::PDAF pdaf; }; struct Output { @@ -87,56 +55,23 @@ template - step(const DynModT& dyn_model, const SensModT& sen_model, double timestep, const Gauss_x& x_est, - const MeasurementsZd& z_meas, const Config& config) static Output step(const DynModT &dyn_model, const SensModT &sen_model, double timestep, const Gauss_x &x_est, const Arr_zXd &z_measurements, const Config &config) { - auto [x_pred, z_pred] = EKF::predict(dyn_model, sen_model, timestep, x_est); - 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) - { - x_updated.push_back(EKF::update(sen_model, x_pred, z_pred, measurement)); + auto [x_pred, z_pred] = EKF::predict(dyn_model, sen_model, timestep, x_est); + auto gated_measurements = apply_gate(z_measurements, z_pred, config); + auto inside_meas = get_inside_measurements(z_measurements, gated_measurements); + Gauss_xX x_updated; for (const auto &z_k : inside_meas.colwise()) { x_updated.push_back(EKF::update(sen_model, x_pred, z_pred, z_k)); } - Gauss_x x_final = - get_weighted_average(inside, 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_meas, x_updated, z_pred, x_pred, config.pdaf.prob_of_detection, config.pdaf.clutter_intensity); + return {x_final, x_pred, z_pred, x_updated, gated_measurements}; } - /** - * @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) + static Arr_1Xb apply_gate(const Arr_zXd &z_measurements, const Gauss_z &z_pred, Config config) { double mahalanobis_threshold = config.pdaf.mahalanobis_threshold; double min_gate_threshold = config.pdaf.min_gate_threshold; @@ -147,27 +82,27 @@ 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) + // Getting association probabilities according to textbook p. 123 "Corollary 7.3.3" + static Eigen::VectorXd get_weights(const Arr_zXd &z_measurements, const Gauss_z &z_pred, double prob_of_detection, double clutter_intensity) { double lambda = clutter_intensity; double P_d = prob_of_detection; @@ -220,15 +144,14 @@ template #include -using ConstantVelocity = vortex::models::ConstantVelocity; +using ConstantVelocity = vortex::models::ConstantVelocity; using IdentitySensorModel = vortex::models::IdentitySensorModel<4, 2>; using PDAF = vortex::filter::PDAF; @@ -205,11 +205,10 @@ TEST(PDAF, apply_gate_is_separating_correctly) vortex::utils::Ellipse prediction = vortex::plotting::gauss_to_ellipse(z_pred, config.pdaf.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) @@ -228,7 +227,12 @@ TEST(PDAF, apply_gate_is_separating_correctly_2) EXPECT_EQ(gated.count(), 5u); -#if (GNUPLOT_ENABLE) + #if (GNUPLOT_ENABLE) + std::vector> meas_vec; + for (Eigen::Vector2d m : meas.colwise()) { + meas_vec.push_back({m(0), m(1)}); + } + Gnuplot gp; gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; gp << "set size ratio -1\n"; @@ -244,11 +248,10 @@ TEST(PDAF, apply_gate_is_separating_correctly_2) vortex::utils::Ellipse prediction = vortex::plotting::gauss_to_ellipse(z_pred, config.pdaf.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 @@ -266,14 +269,20 @@ TEST(PDAF, predict_next_state_is_calculating) Eigen::Array2Xd meas = {{0.0, 1.0, 1.0, 0.0, 2.0, 2.0}, {1.0, 0.0, 1.0, 2.0, 0.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, x_pred, z_pred, x_updated, gated] = 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) + std::vector> meas_vec; + for (Eigen::Vector2d m : meas.colwise()) + { + meas_vec.push_back({m(0), m(1)}); + } + Gnuplot gp; gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; gp << "set size ratio -1\n"; @@ -289,12 +298,10 @@ 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 " @@ -309,13 +316,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::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"; + vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.pdaf.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 + #endif } TEST(PDAF, predict_next_state_2) @@ -335,14 +341,19 @@ TEST(PDAF, predict_next_state_2) {-3.0, 0.0, 1.5, -2.0, 0.0, 1.0}}; // clang-format on - ConstantVelocity dyn_model{ 1.0 }; - IdentitySensorModel sen_model{ 0.5 }; + ConstantVelocity dyn_model{1.0}; + IdentitySensorModel sen_model{0.5}; auto [x_final, x_pred, z_pred, x_updated, gated] = 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) + std::vector> meas_vec; + for (Eigen::Vector2d m : meas.colwise()) { + meas_vec.push_back({m(0), m(1)}); + } + Gnuplot gp; gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; gp << "set size ratio -1\n"; @@ -358,12 +369,10 @@ 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 " @@ -378,13 +387,12 @@ 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::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"; + vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.pdaf.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 + #endif } TEST(PDAF, predict_next_state_3_1) @@ -402,14 +410,19 @@ TEST(PDAF, predict_next_state_3_1) {0.5, 0.2, 2.3, 0.0, 2.7, 2.5}}; // clang-format on - ConstantVelocity dyn_model{ 0.5 }; - IdentitySensorModel sen_model{ 1.0 }; + ConstantVelocity dyn_model{0.5}; + IdentitySensorModel sen_model{1.0}; auto [x_final, x_pred, z_pred, x_updated, gated] = 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) + std::vector> meas_vec; + for (Eigen::Vector2d m : meas.colwise()) { + meas_vec.push_back({m(0), m(1)}); + } + Gnuplot gp; gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; gp << "set size ratio -1\n"; @@ -425,12 +438,10 @@ 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 " @@ -447,13 +458,12 @@ 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::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"; + vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.pdaf.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 + #endif } TEST(PDAF, predict_next_state_3_2) @@ -471,14 +481,18 @@ TEST(PDAF, predict_next_state_3_2) {2.0 , 0.5, 3.4, 1.3, 0.5 , 0.89}}; // clang-format on - ConstantVelocity dyn_model{ 0.5 }; - IdentitySensorModel sen_model{ 1.0 }; + ConstantVelocity dyn_model{0.5}; + IdentitySensorModel sen_model{1.0}; auto [x_final, x_pred, z_pred, x_updated, gated] = 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) + std::vector> meas_vec; + for (Eigen::Vector2d m : meas.colwise()) { + meas_vec.push_back({m(0), m(1)}); + } Gnuplot gp; gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; gp << "set size ratio -1\n"; @@ -494,12 +508,10 @@ 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 " @@ -520,13 +532,12 @@ 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::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"; + vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.pdaf.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 + #endif } TEST(PDAF, predict_next_state_3_3) @@ -543,14 +554,19 @@ TEST(PDAF, predict_next_state_3_3) { 2.5, 2.7, 2.3, 1.9, 3.0, 2.04}}; // clang-format on - ConstantVelocity dyn_model{ 0.5 }; - IdentitySensorModel sen_model{ 1.0 }; + ConstantVelocity dyn_model{0.5}; + IdentitySensorModel sen_model{1.0}; auto [x_final, x_pred, z_pred, x_updated, gated] = 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) + std::vector> meas_vec; + for (Eigen::Vector2d m : meas.colwise()) { + meas_vec.push_back({m(0), m(1)}); + } + Gnuplot gp; gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; gp << "set size ratio -1\n"; @@ -566,12 +582,10 @@ 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 " @@ -596,13 +610,12 @@ 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::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"; + vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.pdaf.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 + #endif } TEST(PDAF, predict_next_state_3_4) @@ -619,14 +632,19 @@ TEST(PDAF, predict_next_state_3_4) { 2.2, 2.3, 2.0, 1.5, 2.0, 2.5}}; // clang-format on - ConstantVelocity dyn_model{ 0.5 }; - IdentitySensorModel sen_model{ 1.0 }; + ConstantVelocity dyn_model{0.5}; + IdentitySensorModel sen_model{1.0}; auto [x_final, x_pred, z_pred, x_updated, gated] = 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) + std::vector> meas_vec; + for (Eigen::Vector2d m : meas.colwise()) { + meas_vec.push_back({m(0), m(1)}); + } + Gnuplot gp; gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; gp << "set size ratio -1\n"; @@ -642,12 +660,10 @@ 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 " @@ -676,36 +692,10 @@ 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::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"; + vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.pdaf.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_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); + #endif } \ No newline at end of file