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 <vector>
 #include <Eigen/Dense>
+#include <vector>
 #include <vortex_filtering/filters/pdaf.hpp>
 #include <vortex_filtering/models/dynamic_model_interfaces.hpp>
 #include <vortex_filtering/models/sensor_model_interfaces.hpp>
 
-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 <concepts::DynamicModelLTVWithDefinedSizes DynModT, concepts::SensorModelLTVWithDefinedSizes SensModT>
-class IPDA
-{
+template <concepts::DynamicModelLTVWithDefinedSizes DynModT, concepts::SensorModelLTVWithDefinedSizes SensModT> 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<N_DIM_x, N_DIM_z, N_DIM_u, N_DIM_v, N_DIM_w>;
 
-  using Arr_zXd    = Eigen::Array<double, N_DIM_z, Eigen::Dynamic>;
-  using Arr_1Xb    = Eigen::Array<bool, 1, Eigen::Dynamic>;
-  using EKF        = vortex::filter::EKF_t<N_DIM_x, N_DIM_z, N_DIM_u, N_DIM_v, N_DIM_w>;
-  using PDAF       = vortex::filter::PDAF<DynModT, SensModT>;
+  using Arr_zXd = Eigen::Array<double, N_DIM_z, Eigen::Dynamic>;
+  using Arr_1Xb = Eigen::Array<bool, 1, Eigen::Dynamic>;
+  using EKF     = vortex::filter::EKF_t<N_DIM_x, N_DIM_z, N_DIM_u, N_DIM_v, N_DIM_w>;
+  using PDAF    = vortex::filter::PDAF<DynModT, SensModT>;
 
   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<Gauss_x, double, std::vector<Vec_z>, std::vector<Vec_z>, Gauss_x, Gauss_z, std::vector<Gauss_x>>
-  step(const DynModT &dyn_mod, const SensModT &sens_mod, double timestep, const Gauss_x &x_est, const std::vector<Vec_z> &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 <Eigen/Dense>
@@ -19,6 +9,7 @@
 #include <vector>
 #include <vortex_filtering/vortex_filtering.hpp>
 
+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 <concepts::DynamicModelLTVWithDefinedSizes DynModT, concepts::SensorModelLTVWithDefinedSizes SensModT>
-class PDAF
-{
 template <concepts::DynamicModelLTVWithDefinedSizes DynModT, concepts::SensorModelLTVWithDefinedSizes SensModT> class PDAF {
 public:
   static constexpr int N_DIM_x = DynModT::N_DIM_x;
@@ -54,27 +35,14 @@ template <concepts::DynamicModelLTVWithDefinedSizes DynModT, concepts::SensorMod
   using Gauss_x    = typename T::Gauss_x;
   using Vec_z      = typename T::Vec_z;
   using GaussMix_x = typename T::GaussMix_x;
-  using GaussMix_z = typename T::GaussMix_z; 
+  using GaussMix_z = typename T::GaussMix_z;
   using Arr_zXd    = Eigen::Array<double, N_DIM_z, Eigen::Dynamic>;
   using Arr_1Xb    = Eigen::Array<bool, 1, Eigen::Dynamic>;
   using Gauss_xX   = std::vector<Gauss_x>;
   using EKF        = vortex::filter::EKF_t<N_DIM_x, N_DIM_z, N_DIM_u, N_DIM_v, N_DIM_w>;
 
-  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 <concepts::DynamicModelLTVWithDefinedSizes DynModT, concepts::SensorMod
 
   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<Gauss_x, MeasurementsZd, MeasurementsZd, Gauss_x, Gauss_z, StatesXd>
-  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<MeasurementsZd, MeasurementsZd> 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 <concepts::DynamicModelLTVWithDefinedSizes DynModT, concepts::SensorMod
     for (size_t a_k = 0; const Vec_z &z_k : z_measurements.colwise()) {
       double mahalanobis_distance = z_pred.mahalanobis_distance(z_k);
       double regular_distance     = (z_pred.mean() - z_k).norm();
-      gated_measurements(a_k++) = (mahalanobis_distance <= mahalanobis_threshold || regular_distance <= min_gate_threshold) && regular_distance <= max_gate_threshold;
+      gated_measurements(a_k++) =
+          (mahalanobis_distance <= mahalanobis_threshold || regular_distance <= min_gate_threshold) && regular_distance <= max_gate_threshold;
     }
     return gated_measurements;
   }
 
-    return { inside_meas, outside_meas };
+  static Arr_zXd get_inside_measurements(const Arr_zXd &z_measurements, const Arr_1Xb &gated_measurements)
+  {
+    Arr_zXd inside_meas(N_DIM_z, gated_measurements.count());
+    for (size_t i = 0, j = 0; bool gated : gated_measurements) {
+      if (gated) {
+        inside_meas.col(j++) = z_measurements.col(i);
+      }
+      i++;
+    }
+    return inside_meas;
   }
 
-  /**
-   * @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)
+  // Getting weighted average of the predicted states
+  static Gauss_x get_weighted_average(const Arr_zXd &z_measurements, const Gauss_xX &updated_states, const Gauss_z &z_pred, const Gauss_x &x_pred,
+                                      double prob_of_detection, double clutter_intensity)
   {
     Gauss_xX states;
     states.push_back(x_pred);
@@ -178,19 +113,8 @@ template <concepts::DynamicModelLTVWithDefinedSizes DynModT, concepts::SensorMod
     return GaussMix_x{weights, states}.reduce();
   }
 
-  /**
-   * @brief Gets the weights for the measurements -> 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 <concepts::DynamicModelLTVWithDefinedSizes DynModT, concepts::SensorMod
     Eigen::ArrayXd weights(m_k + 1);
 
     // Accociation probabilities (Corrolary 7.3.3)
-    weights(0) = lambda * (1 - P_d);         
+    weights(0)        = lambda * (1 - P_d);
     weights.tail(m_k) = P_d * z_likelyhoods;
 
     // normalize weights
     weights /= weights.sum();
 
     return weights;
-
   }
 };
 
-}  // namespace vortex::filter
\ No newline at end of file
+} // namespace vortex::filter
\ No newline at end of file
diff --git a/vortex-filtering/test/pdaf_test.cpp b/vortex-filtering/test/pdaf_test.cpp
index db075071..84296788 100644
--- a/vortex-filtering/test/pdaf_test.cpp
+++ b/vortex-filtering/test/pdaf_test.cpp
@@ -4,7 +4,7 @@
 #include <vortex_filtering/filters/pdaf.hpp>
 #include <vortex_filtering/utils/ellipse.hpp>
 
-using ConstantVelocity = vortex::models::ConstantVelocity;
+using ConstantVelocity    = vortex::models::ConstantVelocity;
 using IdentitySensorModel = vortex::models::IdentitySensorModel<4, 2>;
 using PDAF                = vortex::filter::PDAF<ConstantVelocity, IdentitySensorModel>;
 
@@ -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<std::pair<double, double>> 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<std::pair<double, double>> 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<std::pair<double, double>> 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<std::pair<double, double>> 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<std::pair<double, double>> 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<std::pair<double, double>> 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<std::pair<double, double>> 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<TestDynamicModel, TestSensorModel>;
-
-  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<Eigen::Vector2d> 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