diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
index 68aef9201b513476c1ce9b4e6ee9adc2ebd54397..a244134eef490deec49763ec7feb5a8befb3c789 100644
--- a/.vscode/c_cpp_properties.json
+++ b/.vscode/c_cpp_properties.json
@@ -3,13 +3,13 @@
         {
             "name": "Linux",
             "includePath": [
-                "${workspaceFolder}/include",
-                "/usr/include/eigen3"
+                "/usr/include/eigen3",
+                "${workspaceFolder}/include"
             ],
             "defines": [],
-            "compilerPath": "/usr/bin/clang++-6.0",
+            "compilerPath": "/usr/bin/g++",
             "cStandard": "c11",
-            "cppStandard": "c++17",
+            "cppStandard": "c++11",
             "intelliSenseMode": "clang-x64",
             "compileCommands": "${workspaceFolder}/build/compile_commands.json",
             "configurationProvider": "vector-of-bool.cmake-tools"
diff --git a/.vscode/settings.json b/.vscode/settings.json
index 68622c15fcaceaddf1a0d3119bd21196692ce09a..cb5363037fc66974504e0cffec991f24dab55ad7 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -1,5 +1,5 @@
 {
-  "python.pythonPath": "/usr/bin/python",
+  "python.pythonPath": "/usr/bin/python3",
   "C_Cpp.configurationWarnings": "Disabled",
   "files.associations": {
     "*.mb": "markdown",
diff --git a/conanfile.py b/conanfile.py
index 6a0510a4ff3a182b2ab6152ba884a5d19393a4b5..5d550239eafa738bd2a299b24f3b136e756a9240 100644
--- a/conanfile.py
+++ b/conanfile.py
@@ -3,7 +3,7 @@ from conans import ConanFile, CMake
 
 class FilterBayConan(ConanFile):
     name = "filter_bay"
-    version = "0.4.2"
+    version = "0.4.3"
     license = "MIT"
     url = "https://git.rwth-aachen.de/robo_guide/filter_bay"
     description = ("A small filter library for BAYesian filtering.")
diff --git a/include/filter_bay/particle_filter/log_particle_filter.hpp b/include/filter_bay/particle_filter/log_particle_filter.hpp
index 73e1b021a03e0eda6fe7d285f3f845fae258de02..d8b93bafc5841dabc050ad6f6e6ccf74a193434e 100644
--- a/include/filter_bay/particle_filter/log_particle_filter.hpp
+++ b/include/filter_bay/particle_filter/log_particle_filter.hpp
@@ -1,85 +1,77 @@
 #pragma once
-#include <filter_bay/utility/log_arithmetics.hpp>
-#include <filter_bay/utility/uniform_sampler.hpp>
 #include <algorithm>
 #include <cmath>
+#include <filter_bay/utility/log_arithmetics.hpp>
+#include <filter_bay/utility/uniform_sampler.hpp>
 #include <functional>
 #include <vector>
 
-namespace filter_bay
-{
-/*!
-Particle filter which operates in logarithmic probability domain, see:
-
-Christian Gentner, Siwei Zhang, and Thomas Jost, “Log-PF: Particle Filtering in
-Logarithm Domain,” Journal of Electrical and Computer Engineering, vol. 2018, 
-Article ID 5763461, 11 pages, 2018. https://doi.org/10.1155/2018/5763461.
-
-I simplified the systematic resampling and use the log_exp_sum larger sums
-instead of the iterative jacobian logarithms.
-*/
+namespace filter_bay {
+/**
+ * Particle filter which operates in logarithmic probability domain, see:
+ * Christian Gentner, Siwei Zhang, and Thomas Jost, “Log-PF: Particle Filtering
+ * in Logarithm Domain,” Journal of Electrical and Computer Engineering, vol.
+ * 2018, Article ID 5763461, 11 pages, 2018.
+ * https://doi.org/10.1155/2018/5763461.
+ *
+ * T systematic resampling uses the log_exp_sum larger sums instead of the
+ * iterative jacobian logarithms.
+ */
 template <typename StateType>
-class LogParticleFilter
-{
-public:
-  /*!
-  Create a logarithmic particle with the given number of particles.
-  */
+class LogParticleFilter {
+ public:
+  /**
+   * Create a logarithmic particle with the given number of particles.
+   * @param particle_count the number of particles to initialize
+   */
   LogParticleFilter(size_t particle_count)
-      : log_weights(particle_count), states(particle_count)
-  {
-    this->particle_count = particle_count;
-  }
-
-  /*!
-  Set the initial state belief. Must have the size particle_count. Distributes 
-  the weights uniformly.
-  */
-  void initialize(std::vector<StateType> initial_states)
-  {
-    if (initial_states.size() == particle_count)
-    {
+      : log_weights(particle_count),
+        states(particle_count),
+        particle_count(particle_count) {}
+
+  /**
+   * Set the initial state belief. Must have the size particle_count.
+   * Distributes the weights uniformly
+   * @param inital_states possible initial states
+   */
+  void initialize(std::vector<StateType> initial_states) {
+    if (initial_states.size() == particle_count) {
       states = std::move(initial_states);
       // log(1/belief_size) = -log(belief_size)
       double log_avg = -log(particle_count);
-      for (double &current : log_weights)
-      {
+      for (double &current : log_weights) {
         current = log_avg;
       }
     }
   }
 
-  /*!
-  Set the predictions of the transition model
-  */
-  void set_predictions(std::vector<StateType> predictions)
-  {
+  /**
+   * Set the priors of the filter step.
+   * @param predictions prior states
+   */
+  void set_predictions(std::vector<StateType> predictions) {
     states = std::move(predictions);
   }
 
-  /*!
-  Updates the particle weights by incorporating the observation as a batch.
-  The update step is done in a SIR bootstrap filter fashion.
-  Resampling is performed with the low variance resampling method.
-  \param log_likelihoods the logarithmic likelihoods for all the states (in the 
-  same order as these states)
-  */
-  void update(const std::vector<double> &log_likelihoods)
-  {
+  /**
+   * Updates the particle weights by incorporating the observation as a batch.
+   * The update step is done in a SIR bootstrap filter fashion. Resampling is
+   * performed with the low variance resampling method.
+   * @param log_likelihoods the logarithmic likelihoods for all the states (in
+   * the same order as these states)
+   */
+  void update(const std::vector<double> &log_likelihoods) {
     double max_weight = -std::numeric_limits<double>::infinity();
-    max_log_likelihood = -std::numeric_limits<double>::infinity();
+    max_log_like = -std::numeric_limits<double>::infinity();
     // weights as posterior of observation, prior is proposal density
-    for (size_t i = 0; i < particle_count; i++)
-    {
+    for (size_t i = 0; i < particle_count; i++) {
       // log(weight*likelihood) = log(weight) + log_likelihood
       log_weights[i] += log_likelihoods[i];
-      if (log_likelihoods[i] > max_log_likelihood)
-      {
-        max_log_likelihood = log_likelihoods[i];
+      if (log_likelihoods[i] > max_log_like) {
+        max_log_like = log_likelihoods[i];
       }
       // check for MAP here, after resampling the weights are all equal
-      if (log_weights[i] > max_weight)
-      {
+      if (log_weights[i] > max_weight) {
         map_state = states[i];
         max_weight = log_weights[i];
       }
@@ -88,118 +80,70 @@ public:
     log_weights = normalized_logs(log_weights);
     // Perform resampling? Effective sample_size < half of particle count
     double log_resample_threshold = std::log(particle_count / 2.0);
-    log_effective_samples_size = log_effective_samples(log_weights);
-    if (log_effective_samples_size < log_resample_threshold)
-    {
-      resample_systematic();
+    log_n_eff = log_effective_sample_size(log_weights);
+    if (log_n_eff < log_resample_threshold) {
+      resample();
     }
   }
 
-  /*!
-  Returns the maximum-a-posteriori state from the last update step.
-  */
-  StateType get_map_state() const
-  {
-    return map_state;
-  }
-
-  /*!
-  Returns the effective sample size. This is calculated before resampling!
-  */
-  double effective_sample_size() const
-  {
-    return std::exp(log_effective_samples_size);
-  }
-
-  /*!
-  Returns maximum log-likelihood from the last update step
-  */
-  double max_log_liklihood() const
-  {
-    return max_log_likelihood;
-  }
-
-  /*!
-  Returns the logarithmic effective sample size from the weights
-  \param log_weight_vec logarithmic weights
-  */
-  static double log_effective_samples(
-      const std::vector<double> &log_weight_vec)
-  {
+  /**
+   * Returns the maximum-a-posteriori state from the last update step.
+   */
+  StateType get_map_state() const { return map_state; }
+
+  /**
+   * Returns the effective sample size. This is calculated before resampling!
+   */
+  double effective_sample_size() const { return std::exp(log_n_eff); }
+
+  /**
+   * Returns maximum log-likelihood from the last update step
+   */
+  double max_log_likelihood() const { return max_log_like; }
+
+  /**
+   * Returns the logarithmic effective sample size from the weights
+   * @param log_weight_vec logarithmic weights
+   */
+  static double log_effective_sample_size(
+      const std::vector<double> &log_weight_vec) {
     return log_effective_samples_impl(log_weight_vec);
   }
 
-  /*! 
-  Returns the state of the particles. Use get_log_weights to obtain the full
-  belief.
-  */
-  const std::vector<StateType> &get_states() const
-  {
-    return states;
-  }
-
-  /*! 
-  Returns the logarithmic weights of the particles. Use get_states to obtain the
-  full belief.
-  Remember: after resampling all weights are the same
-  */
-  const std::vector<double> &get_log_weights() const
-  {
-    return log_weights;
-  }
-
-  /*! 
-  Calculates the weights from the logarithmic weights. Use get_states to obtain
-  the full belief.
-  Remember: after resampling all weights are the same
-  */
-  std::vector<double> get_weights() const
-  {
+  /**
+   * Returns the state of the particles. Use get_log_weights to obtain the full
+   * belief.
+   */
+  const std::vector<StateType> &get_states() const { return states; }
+
+  /**
+   * Returns the logarithmic weights of the particles. Use get_states to obtain
+   * the full belief.
+   */
+  const std::vector<double> &get_log_weights() const { return log_weights; }
+
+  /**
+   * Calculates the weights from the logarithmic weights. Use get_states to
+   * obtain the full belief.
+   */
+  std::vector<double> get_weights() const {
     std::vector<double> weights;
     weights.reserve(log_weights.size());
-    for (double value : log_weights)
-    {
+    for (double value : log_weights) {
       weights.push_back(std::exp(value));
     }
     return weights;
   }
 
-  /*! Returns the number of particles beeing simulated */
-  size_t get_particle_count() const
-  {
-    return particle_count;
-  }
-
-  /*! 
-  Set the number of simulated particles. The particles are resized and it is
-  advised to call initialize after setting the particle count.
-  */
-  void set_particle_count(size_t count)
-  {
-    particle_count = count;
-    states.resize(particle_count);
-    log_weights.resize(particle_count);
-  }
+  /**
+   * Returns the number of particles beeing simulated
+   */
+  size_t get_particle_count() const { return particle_count; }
 
-private:
-  // corrsponding states and weights
-  std::vector<double> log_weights;
-  std::vector<StateType> states;
-  // parameters
-  size_t particle_count;
-  // maximum-a-posteriori
-  StateType map_state;
-  double max_log_likelihood;
-  double log_effective_samples_size;
-  // Uniform random number generator
-  UniformSampler uniform_sampler;
-
-  /*!
-  Sampling systematically and thus keeping the sample variance lower than pure
-  random sampling. It is also faster O(m) instead of O(m logm)
-  */
-  void resample_systematic()
-  {
+  /**
+   * Systematic resampling of the particles.
+   */
+  void resample() {
     // Make copy of old belief
     std::vector<StateType> old_states = states;
     std::vector<double> old_weights = log_weights;
@@ -208,13 +152,11 @@ private:
     double avg_weight = 1.0 / particle_count;
     double start_weight = uniform_sampler(0, avg_weight);
     double log_avg = log(avg_weight);
-    // o in old_weights, n in new_weigts
+    // o in old_weights, n in new_weights
     size_t o = 0;
-    for (size_t n = 0; n < particle_count; n++)
-    {
+    for (size_t n = 0; n < particle_count; n++) {
       double U = log(start_weight + n * avg_weight);
-      while (U > cumulative)
-      {
+      while (U > cumulative) {
         o++;
         cumulative = jacobi_logarithm(cumulative, old_weights[o]);
       }
@@ -223,5 +165,28 @@ private:
       log_weights[n] = log_avg;
     }
   }
-};
-} // namespace filter_bay
\ No newline at end of file
+
+  /**
+   * Set the number of simulated particles. The particles are resized and it
+   * is advised to call initialize after setting the particle count.
+   */
+  void set_particle_count(size_t count) {
+    particle_count = count;
+    states.resize(particle_count);
+    log_weights.resize(particle_count);
+  }
+
+ private:
+  // corrsponding states and weights
+  std::vector<double> log_weights;
+  std::vector<StateType> states;
+  // parameters
+  size_t particle_count;
+  // maximum-a-posteriori
+  StateType map_state;
+  double max_log_like;
+  double log_n_eff;
+  // Uniform random number generator
+  UniformSampler uniform_sampler;
+};  // namespace filter_bay
+}  // namespace filter_bay
diff --git a/include/filter_bay/particle_filter/particle_filter.hpp b/include/filter_bay/particle_filter/particle_filter.hpp
index 33abd2288c058513bfbd03159b618adf1cedf03f..e00cb6b99a5c3887906b35bb65e030530c1ed874 100644
--- a/include/filter_bay/particle_filter/particle_filter.hpp
+++ b/include/filter_bay/particle_filter/particle_filter.hpp
@@ -1,77 +1,69 @@
 #pragma once
-#include <filter_bay/utility/uniform_sampler.hpp>
 #include <algorithm>
 #include <cmath>
+#include <filter_bay/utility/uniform_sampler.hpp>
 #include <functional>
 #include <vector>
 
-namespace filter_bay
-{
-/*!
-Simple implementation of a particle filter which uses systematic resampling.
-*/
+namespace filter_bay {
+/**
+ * A particle filter which uses systematic resampling.
+ */
 template <typename StateType>
-class ParticleFilter
-{
-public:
-  /*!
-  Create a particle with the given number of particles.
-  */
+class ParticleFilter {
+ public:
+  /**
+   * Create a logarithmic particle with the given number of particles.
+   * @param particle_count the number of particles to initialize
+   */
   ParticleFilter(size_t particle_count)
-      : weights(particle_count), states(particle_count)
-  {
-    this->particle_count = particle_count;
-  }
+      : weights(particle_count),
+        states(particle_count),
+        particle_count(particle_count) {}
 
-  /*!
-  Set the initial belief. Must have the size particle_count. Makes sure that the
-  initial weights are distributed uniformally.
-  */
-  void initialize(std::vector<StateType> initial_states)
-  {
-    if (initial_states.size() == particle_count)
-    {
+  /**
+   * Set the initial state belief. Must have the size particle_count.
+   * Distributes the weights uniformly
+   * @param inital_states possible initial states
+   */
+  void initialize(std::vector<StateType> initial_states) {
+    if (initial_states.size() == particle_count) {
       states = std::move(initial_states);
       double avg_weight = 1.0 / particle_count;
-      for (double &current : weights)
-      {
+      for (double &current : weights) {
         current = avg_weight;
       }
     }
   }
 
-  /*!
-  Set the predictions of the transition model
-  */
-  void set_predictions(std::vector<StateType> predictions)
-  {
+  /**
+   * Set the priors of the filter step.
+   * @param predictions prior states
+   */
+  void set_predictions(std::vector<StateType> predictions) {
     states = std::move(predictions);
   }
 
-  /*!
-  Updates the particle weights by incorporating the observation as a batch.
-  The update step is done in a SIR bootstrap filter fashion.
-  Resampling is performed with the low variance resampling method.
-  \param likelihoods the likelihoods for all the states (in the same order as 
-  these states)
-  */
-  void update(const std::vector<double> &likelihoods)
-  {
+  /**
+   * Updates the particle weights by incorporating the observation as a batch.
+   * The update step is done in a SIR bootstrap filter fashion. Resampling is
+   * performed with the low variance resampling method.
+   * @param likelihoods the logarithmic likelihoods for all the states (in
+   * the same order as these states)
+   */
+  void update(const std::vector<double> &likelihoods) {
     double weight_sum = 0;
     double max_weight = -std::numeric_limits<double>::infinity();
-    max_likelihood = -std::numeric_limits<double>::infinity();
+    max_like = -std::numeric_limits<double>::infinity();
     // weights as posterior of observation
-    for (size_t i = 0; i < particle_count; i++)
-    {
+    for (size_t i = 0; i < particle_count; i++) {
       // Using the prior as proposal so the weight recursion is simply:
       weights[i] *= likelihoods[i];
-      if (likelihoods[i] > max_likelihood)
-      {
-        max_likelihood = likelihoods[i];
+      if (likelihoods[i] > max_like) {
+        max_like = likelihoods[i];
       }
       // check for MAP here, after resampling the weights are all equal
-      if (weights[i] > max_weight)
-      {
+      if (weights[i] > max_weight) {
         map_state = states[i];
         max_weight = weights[i];
       }
@@ -79,110 +71,59 @@ public:
     }
     // Normalize weights
     double normalize_const = 1 / weight_sum;
-    for (auto &current : weights)
-    {
+    for (auto &current : weights) {
       current *= normalize_const;
     }
     // resample if effective sample size is smaller than the half of particle
     // count
-    effective_samples_size = effective_samples(weights);
-    if (effective_samples_size < particle_count / 2.0)
-    {
-      resample_systematic();
+    n_eff = effective_sample_size(weights);
+    if (n_eff < particle_count / 2.0) {
+      resample();
     }
   }
 
-  /*!
-  Returns the effective sample size. This is calculated before resampling!
-  */
-  double effective_sample_size() const
-  {
-    return effective_samples_size;
-  }
+  /**
+   * Returns the maximum-a-posteriori state from the last update step.
+   */
+  StateType get_map_state() const { return map_state; }
 
-  /*!
-  Returns the effective sample size from the weights
-  */
-  static double effective_samples(const std::vector<double> &weight_vec)
-  {
+  /**
+   * Returns the effective sample size.
+   */
+  static double effective_sample_size(const std::vector<double> &weight_vec) {
     double sum_of_squares = 0;
-    for (double value : weight_vec)
-    {
+    for (double value : weight_vec) {
       sum_of_squares += value * value;
     }
     return 1.0 / sum_of_squares;
   }
 
-  /*!
-  Returns maximum likelihood from the last update step
-  */
-  double max_liklihood() const
-  {
-    return max_likelihood;
-  }
-
-  /*!
-  Returns the maximum-a-posteriori state from the last update step.
-  */
-  StateType get_map_state() const
-  {
-    return map_state;
-  }
-
-  /*! 
-  Returns the state of the particles. Use get_weights to obtain the full belief.
-  */
-  const std::vector<StateType> &get_states() const
-  {
-    return states;
-  }
-
-  /*! 
-  Returns the  weights of the particles. Use get_states to obtain the full
-  belief.
-  Remember: after resampling all weight are the same
-  */
-  const std::vector<double> &get_weights() const
-  {
-    return weights;
-  }
+  /**
+   * Returns maximum log-likelihood from the last update step
+   */
+  double max_likelihood() const { return max_like; }
 
-  /*! Returns the number of particles beeing simulated */
-  size_t get_particle_count() const
-  {
-    return particle_count;
-  }
+  /**
+   * Returns the state of the particles. Use get_log_weights to obtain the full
+   * belief.
+   */
+  const std::vector<StateType> &get_states() const { return states; }
 
-  /*! 
-  Set the number of simulated particles. The particles are resized and it is
-  advised to call initialize after setting the particle count.
-  */
-  void set_particle_count(size_t count)
-  {
-    particle_count = count;
-    states.resize(particle_count);
-    weights.resize(particle_count);
-  }
+  /**
+   * Returns the weights of the particles. Use get_states to obtain the full
+   * belief.
+   */
+  const std::vector<double> &get_weights() const { return weights; }
 
-private:
-  // corrsponding states and weights
-  std::vector<double> weights;
-  std::vector<StateType> states;
-  // parameters
-  size_t particle_count;
-  // maximum-a-posteriori state;
-  StateType map_state;
-  double effective_samples_size;
-  double max_likelihood;
-  // sample from uniform distribution
-  UniformSampler uniform_sampler;
+  /**
+   * Returns the number of particles beeing simulated
+   */
+  size_t get_particle_count() const { return particle_count; }
 
-  /*!
-  Sampling systematically and thus keeping the sample variance lower than pure
-  random sampling. It is also faster O(m) instead of O(m logm)
-  */
-  void resample_systematic()
-  {
+  /**
+   * Systematic resampling of the particles.
+   */
+  void resample() {
     // Make copy of old belief
     std::vector<StateType> old_states = states;
     std::vector<double> old_weights = weights;
@@ -192,11 +133,9 @@ private:
     double start_weight = uniform_sampler(0, avg_weight);
     // indices: o in old, n in new
     size_t o = 0;
-    for (size_t n = 0; n < particle_count; n++)
-    {
+    for (size_t n = 0; n < particle_count; n++) {
       double U = start_weight + n * avg_weight;
-      while (U > cumulative)
-      {
+      while (U > cumulative) {
         o++;
         cumulative += old_weights[o];
       }
@@ -205,5 +144,28 @@ private:
       weights[n] = avg_weight;
     }
   }
+
+  /**
+   * Set the number of simulated particles. The particles are resized and it
+   * is advised to call initialize after setting the particle count.
+   */
+  void set_particle_count(size_t count) {
+    particle_count = count;
+    states.resize(particle_count);
+    weights.resize(particle_count);
+  }
+
+ private:
+  // corrsponding states and weights
+  std::vector<double> weights;
+  std::vector<StateType> states;
+  // parameters
+  size_t particle_count;
+  // maximum-a-posteriori state;
+  StateType map_state;
+  double n_eff;
+  double max_like;
+  // sample from uniform distribution
+  UniformSampler uniform_sampler;
 };
-} // namespace filter_bay
\ No newline at end of file
+}  // namespace filter_bay
\ No newline at end of file
diff --git a/include/filter_bay/utility/log_arithmetics.hpp b/include/filter_bay/utility/log_arithmetics.hpp
index 198960f883083ab3e78de9fafab061859829a59c..07893d202281375e97075a6f0740f78042f94e6c 100644
--- a/include/filter_bay/utility/log_arithmetics.hpp
+++ b/include/filter_bay/utility/log_arithmetics.hpp
@@ -5,90 +5,77 @@
 #include <stdexcept>
 #include <vector>
 
-namespace filter_bay
-{
-/*!
-Calculates ln(exp(a) + exp(b)) while avoiding overflow and underflow issues.
-*/
-inline double jacobi_logarithm(double log_a, double log_b)
-{
-  return std::max(log_a, log_b) + std::log(1 +
-                                           std::exp(-std::abs(log_a - log_b)));
+namespace filter_bay {
+/**
+ * Calculates ln(exp(a) + exp(b)) while avoiding overflow and underflow issues.
+ */
+inline double jacobi_logarithm(double log_a, double log_b) {
+  return std::max(log_a, log_b) +
+         std::log(1 + std::exp(-std::abs(log_a - log_b)));
 }
 
-/*!
-Calculates the  logarithmic sum of exponentials ln(sum(exp(x_i))) while avoiding 
-overflow and underflow issues.
-*/
+/**
+ * Calculates the  logarithmic sum of exponentials ln(sum(exp(x_i))) while
+ * avoiding overflow and underflow issues.
+ */
 template <size_t N>
-inline double log_sum_exp(const std::array<double, N> &log_values)
-{
+inline double log_sum_exp(const std::array<double, N> &log_values) {
   double max = *std::max_element(log_values.begin(), log_values.end());
   double exp_sum = 0;
-  for (double current : log_values)
-  {
+  for (double current : log_values) {
     exp_sum += std::exp(current - max);
   }
   return max + std::log(exp_sum);
 }
 
-/*!
-Calculates the  logarithmic sum of exponentials ln(sum(exp(x_i))) while avoiding 
-overflow and underflow issues.
-*/
-inline double log_sum_exp(const std::vector<double> &log_values)
-{
+/**
+ * Calculates the  logarithmic sum of exponentials ln(sum(exp(x_i))) while
+ * avoiding overflow and underflow issues.
+ */
+inline double log_sum_exp(const std::vector<double> &log_values) {
   double max = *std::max_element(log_values.begin(), log_values.end());
   double exp_sum = 0;
-  for (double current : log_values)
-  {
+  for (double current : log_values) {
     exp_sum += std::exp(current - max);
   }
   return max + std::log(exp_sum);
 }
 
-/*!
-Calculates the normalized values in logarithmic domain:
-norm_val = val - normalization_const . Instead of norm_val = val / norm_const
-\param log_values the unnormalizedvalues in logarithmic domain
-*/
+/** Calculates the normalized values in logarithmic domain: norm_val = val -
+ * normalization_const . Instead of norm_val = al / norm_const
+ * @param log_values the unnormalized values in logarithmic domain
+ */
 template <size_t N>
-inline std::array<double, N> normalized_logs(std::array<double, N> log_values)
-{
+inline std::array<double, N> normalized_logs(std::array<double, N> log_values) {
   double norm_const = log_sum_exp(log_values);
-  for (double &value : log_values)
-  {
+  for (double &value : log_values) {
     // log(x/N) = log(x) - log(N)
     value -= norm_const;
   }
   return log_values;
 }
 
-/*!
-Calculates the normalized values in logarithmic domain:
-norm_val = val - normalization_const . Instead of norm_val = val / norm_const
-\param log_values the unnormalizedvalues in logarithmic domain
-*/
-inline std::vector<double> normalized_logs(std::vector<double> log_values)
-{
+/**
+ * Calculates the normalized values in logarithmic domain: norm_val = val -
+ * normalization_const . Instead of norm_val = val / norm_const
+ * @param log_values the unnormalized values in logarithmic domain
+ */
+inline std::vector<double> normalized_logs(std::vector<double> log_values) {
   // log(X) where X is the sum over all x
   double norm_const = log_sum_exp(log_values);
-  for (double &value : log_values)
-  {
+  for (double &value : log_values) {
     // log(x/X) = log(x) - log(X)
     value -= norm_const;
   }
   return log_values;
 }
 
-/*!
-Calculates the logarithmic effective sample size.
-\param norm_log_weights the normalized log weights
+/**
+ * Calculates the logarithmic effective sample size.
+ * @param norm_log_weights the normalized log weights
 */
-inline double log_effective_samples_impl(std::vector<double> norm_log_weights)
-{
-  for (double &current : norm_log_weights)
-  {
+inline double log_effective_samples_impl(std::vector<double> norm_log_weights) {
+  for (double &current : norm_log_weights) {
     // log(x^2) = 2log(x)
     current *= 2;
   }
@@ -96,20 +83,17 @@ inline double log_effective_samples_impl(std::vector<double> norm_log_weights)
   return -log_sum_exp(norm_log_weights);
 }
 
-/*!
-Calculates the logarithmic effective sample size.
-\param norm_log_weights the normalized log weights
+/** Calculates the logarithmic effective sample size.
+ * @param norm_log_weights the normalized log weights
 */
 template <size_t N>
-inline double log_effective_samples_impl(std::array<double, N> norm_log_weights)
-{
-  for (double &current : norm_log_weights)
-  {
+inline double log_effective_samples_impl(
+    std::array<double, N> norm_log_weights) {
+  for (double &current : norm_log_weights) {
     // log(x^2)=2*log(x)
     current *= 2;
   }
   // log(1/sum)=-log(sum)
   return -log_sum_exp(norm_log_weights);
 }
-
-} // namespace filter_bay
\ No newline at end of file
+}  // namespace filter_bay
diff --git a/include/filter_bay/utility/normal_sampler.hpp b/include/filter_bay/utility/normal_sampler.hpp
index d747fb983c9f6c0286e24d8ed2976cef42e21bea..cdfd8a2413610cebd01e31b07bf338b8f293ca99 100644
--- a/include/filter_bay/utility/normal_sampler.hpp
+++ b/include/filter_bay/utility/normal_sampler.hpp
@@ -1,51 +1,47 @@
 #pragma once
-#include <functional>
-#include <random>
+#include <Eigen/Cholesky>
 #include <Eigen/Core>
 #include <Eigen/Dense>
-#include <Eigen/Cholesky>
+#include <functional>
+#include <random>
 
-namespace filter_bay
-{
-/*!
-Simplifies drawing from a multivariant normal distribution.
-*/
-class NormalSampler
-{
+namespace filter_bay {
+/**
+ * Simplifies drawing from a multivariant normal distribution.
+ */
+class NormalSampler {
   // Drawing from multivariate normal distribution:
   // https://en.wikipedia.org/wiki/Multivariate_normal_distribution#Drawing_values_from_the_distribution
   // Sample by projecting standard normal distributed values with L, where
   // L L^T = covariance
-public:
+ public:
   // typedef Eigen::Matrix<double, dim, dim> MatrixDim;
   // typedef Eigen::Matrix<double, dim, 1> VectorDim;
 
-  /*!
-  Create the sampler using a random_device to create the seed.
-  */
-  NormalSampler()
-  {
+  /**
+   *   Create the sampler using a random_device to create the seed.
+   */
+  NormalSampler() {
     std::random_device random_device;
     random_generator = std::mt19937(random_device());
   }
 
-  /*!
-  Create the sampler via a seed. For testing it should always be the same seed.
-  In real world use the random_device class to generate the seed.
-  */
+  /**
+   * Create the sampler via a seed. For testing it should always be the same
+   * seed. In real world use the random_device class to generate the seed.
+   */
   NormalSampler(unsigned int seed) : random_generator(seed) {}
 
-  /*!
-  Samples a random vector from a normal distribution using a robust 
-  decomposition algorithm.
-  \param mean the mean of the distribution
-  \param covariance the covariance of the distribution
-  */
+  /**
+   * Samples a random vector from a normal distribution using a robust
+   * decomposition algorithm.
+   * @param mean the mean of the distribution
+   * @param covariance the covariance of the distribution
+   */
   template <typename scalar, int dim>
   auto sample_robust(const Eigen::Matrix<scalar, dim, 1> &mean,
                      const Eigen::Matrix<scalar, dim, dim> &covariance)
-      -> Eigen::Matrix<scalar, dim, 1>
-  {
+      -> Eigen::Matrix<scalar, dim, 1> {
     // Use LDLT decomposition which works with semi definite covariances:
     auto ldlt = covariance.ldlt();
     // L = P^T L sqrt(D), where D is a diagonal matrix.
@@ -58,65 +54,58 @@ public:
     return sample<scalar, dim>(mean, decomposed);
   }
 
-  /*!
-  Samples a random vector from a normal distribution using the Cholesky
-  decomposition. It might fail, if the covariance is not positive definite.
-  \param mean the mean of the distribution
-  \param covariance the covariance of the distribution
-  */
+  /** Samples a random vector from a normal distribution using the Cholesky
+   * decomposition. It might fail, if the covariance is not positive definite.
+   * @param mean the mean of the distribution
+   * @param covariance the covariance of the distribution
+   */
   template <typename scalar, int dim>
   auto sample_cholesky(const Eigen::Matrix<scalar, dim, 1> &mean,
                        const Eigen::Matrix<scalar, dim, dim> &covariance)
-      -> Eigen::Matrix<scalar, dim, 1>
-  {
+      -> Eigen::Matrix<scalar, dim, 1> {
     auto decomposed = covariance.llt().matrixL();
     return sample<scalar, dim>(mean, decomposed);
   }
 
-  /*!
-  Draws random values from a normal distribution parametrized by the mean and
-  standard deviation (NOT variance as in the covariance matrix version).
-  \param mean the mean of the distribution
-  \param standard_deviation the standard deviation of the distribution
-  */
+  /** Draws random values from a normal distribution parametrized by the mean
+   * and standard deviation (NOT variance as in the covariance matrix version).
+   * @param mean the mean of the distribution
+   * @param standard_deviation the standard deviation of the distribution
+   */
   template <typename scalar>
-  scalar draw_normal(scalar mean, scalar standard_deviation)
-  {
+  scalar draw_normal(scalar mean, scalar standard_deviation) {
     std::normal_distribution<scalar> normal_dist(mean, standard_deviation);
     return normal_dist(random_generator);
   }
 
-private:
+ private:
   // Algorithm that provides uniform distributed pseudo-random numbers.
   // Real randomness is imposed via a random seed.
   std::mt19937 random_generator;
 
-  /*!
-  Samples with the given mean and decomposed covariance matrix.
-  */
+  /**
+   * Samples with the given mean and decomposed covariance matrix.
+   */
   template <typename scalar, int dim>
   auto sample(const Eigen::Matrix<scalar, dim, 1> &mean,
               const Eigen::Matrix<scalar, dim, dim> &decomposed)
-      -> Eigen::Matrix<scalar, dim, 1>
-  {
+      -> Eigen::Matrix<scalar, dim, 1> {
     auto norm_dist = create_normal_dist_vector<scalar, dim>();
     return mean + decomposed * norm_dist;
   }
 
-  /*!
-  Creates a vector with all elements drawn randlomly from a standard normal
-  distribution.
-  */
+  /**
+   * Creates a vector with all elements drawn randomly from a standard normal
+   * distribution.
+   */
   template <typename scalar, int dim>
-  Eigen::Matrix<scalar, dim, 1> create_normal_dist_vector()
-  {
+  Eigen::Matrix<scalar, dim, 1> create_normal_dist_vector() {
     std::normal_distribution<scalar> normal_dist;
     Eigen::Matrix<scalar, dim, 1> result;
-    for (int i = 0; i < dim; i++)
-    {
+    for (int i = 0; i < dim; i++) {
       result(i) = normal_dist(random_generator);
     }
     return result;
   }
 };
-} // namespace filter_bay
\ No newline at end of file
+}  // namespace filter_bay
\ No newline at end of file
diff --git a/include/filter_bay/utility/particle_replacer.hpp b/include/filter_bay/utility/particle_replacer.hpp
index 407b73f9ca3406981549ff2835ae3ebddca9ff7e..9190561bd9ba8f09456e5554aee03b791605d5f5 100644
--- a/include/filter_bay/utility/particle_replacer.hpp
+++ b/include/filter_bay/utility/particle_replacer.hpp
@@ -4,9 +4,9 @@
 
 namespace filter_bay {
 
-template <typename StateType>
 class ParticleReplacer {
  private:
+  std::mt19937 generator;
   UniformSampler uniform_sampler;
 
  public:
@@ -15,13 +15,15 @@ class ParticleReplacer {
    */
   ParticleReplacer() {
     std::random_device random_device;
-    uniform_sampler = UniformRandom(random_device());
+    generator = std::mt19937(random_device());
+    uniform_sampler = UniformSampler(generator);
   }
   /**
    * Create the replacer with a custom random seed, intended for testing.
    * @param seed the custom random seed
    */
-  ParticleReplacer(unsigned int seed) : uniform_sampler(seed) {}
+  ParticleReplacer(unsigned int seed)
+      : generator(seed), uniform_sampler(generator) {}
 
   /**
    * Randomly uses either an old or a new state at each position
@@ -31,38 +33,48 @@ class ParticleReplacer {
    * @param new_states will be partially used to replace old states
    * @param ratio the amount of new states in the result
    */
-  std::vector<StateType> replace_random(
-      std::vector<StateType> old_states,
-      const std::vector<StateType>& new_states, double ratio) {
-    for (size_t i = 0; i < old_states.size && i < new_states.size(); i++) {
-      if (uniform_sampler(0, 1) < ratio) {
-        old_states[i] = new_states[i];
-      }
+  template <typename StateType>
+  std::vector<StateType> replace(std::vector<StateType> old_states,
+                                 const std::vector<StateType>& new_states,
+                                 double ratio) {
+    for (size_t i = 0; i < old_states.size() && i < new_states.size(); i++) {
+      old_states[i] = select(old_states[i], new_states[i], ratio);
     }
     return old_states;
-  };
+  }
 
   /**
-   * Shuffles both vectors and replaces the first states of the old states
-   * according to the ratio.
+   * Randomly uses either an old or a new state at each position
+   * The states must have the equal weights so execute this function after
+   * resampling.
    * @param old_states will be partially replaced by new states
-   * @param new_states will be partially used to replace old states
+   * @param new_state will be partially used to replace old states
    * @param ratio the amount of new states in the result
    */
-  std::vector<StateType> replace_shuffle(
-      std::vector<StateType> old_states,
-      const std::vector<StateType>& new_states, double ratio) {
-    // shuffle old and new states
-    std::random_shuffle(old_states.begin(), old_states.end(),
-                        [this] { return this->uniform_sampler() });
-    std::random_shuffle(new_states.begin(), new_states.end(),
-                        [this] { return this->uniform_sampler() });
-    // replace number according ration
-    size_t n_replace = ratio * old_states.size();
-    for (size_t i = 0; i < n_replace && i < new_states.size(); i++) {
-      old_states[i] = new_states[i];
+  template <typename StateType>
+  std::vector<StateType> replace(std::vector<StateType> old_states,
+                                 const StateType& new_state, double ratio) {
+    for (size_t i = 0; i < old_states.size(); i++) {
+      old_states[i] = select(old_states[i], new_state, ratio);
     }
     return old_states;
   }
+
+  /**
+   * Returns the new state with a probability of ratio
+   * @param old_state selected with (probability - 1)
+   * @param new_state selected with probability
+   * @param probability element of [0;1]
+   */
+  template <typename StateType>
+  StateType select(StateType old_state, StateType new_state,
+                   double probability) {
+    auto val = uniform_sampler(0, 1);
+    if (val < probability) {
+      return new_state;
+    } else {
+      return old_state;
+    }
+  }
 };
 }  // namespace filter_bay
diff --git a/include/filter_bay/utility/uniform_sampler.hpp b/include/filter_bay/utility/uniform_sampler.hpp
index 462e822b72d1edc82dfe1ed2b87389a430a1ff2d..675ffb37905846a2cd5460ffafaf8e337fdc80f6 100644
--- a/include/filter_bay/utility/uniform_sampler.hpp
+++ b/include/filter_bay/utility/uniform_sampler.hpp
@@ -18,6 +18,12 @@ class UniformSampler {
    */
   UniformSampler(unsigned int seed) : generator(seed) {}
 
+  /**
+   * Create a uniform random sample with a custom generator (for testing)
+   */
+  UniformSampler(std::mt19937 gen) : generator(gen) {}
+
+
   /*!
   Generates a float random value x with lower<=x<upper
   */
@@ -33,16 +39,6 @@ class UniformSampler {
     return generate(lower, upper);
   }
 
-  /**
-   * Generates a random unsigned integer via the mt19937 generator
-   */
-  uint_fast32_t generate() { return generator(); }
-
-  /**
-   * Generates a random unsigned integer via the mt19937 generator
-   */
-  uint_fast32_t operator()() { return generate(); }
-
  private:
   std::mt19937 generator;
 };
diff --git a/package.xml b/package.xml
index 91fa7efbbd2086be5c691ed44fc3df37629d7c7d..05ec2dfa2e5d35ecfdf1321763268e985ee281f5 100644
--- a/package.xml
+++ b/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>filter_bay</name>
-  <version>0.4.1</version>
+  <version>0.4.3</version>
   <description>
   A small filter library for BAYesian filtering. It does not try to be an
   "Eierlegende Wollmilchsau" as it accepts that the filter models are just
diff --git a/test/log_particle_filter_test.cpp b/test/log_particle_filter_test.cpp
index a0b7e72c02108ada8bb618eeb71e4e63b8c878f5..9d6da0b422c8d0ed69d9a2ea7a2d10b18ebd8427 100644
--- a/test/log_particle_filter_test.cpp
+++ b/test/log_particle_filter_test.cpp
@@ -124,17 +124,17 @@ TEST(LogParticleFilterTest, TestEffectiveSamplesSize)
   {
     log_weights.push_back(std::log(value));
   }
-  ASSERT_DOUBLE_EQ(std::log(3.), filter.log_effective_samples(log_weights));
-  ASSERT_DOUBLE_EQ(3., std::exp(filter.log_effective_samples(log_weights)));
+  ASSERT_DOUBLE_EQ(std::log(3.), filter.log_effective_sample_size(log_weights));
+  ASSERT_DOUBLE_EQ(3., std::exp(filter.log_effective_sample_size(log_weights)));
   weights = {0.2, 0.3, 0.5};
   log_weights = {};
   for (double value : weights)
   {
     log_weights.push_back(std::log(value));
   }
-  ASSERT_NEAR(std::log(2.631578947), filter.log_effective_samples(log_weights),
+  ASSERT_NEAR(std::log(2.631578947), filter.log_effective_sample_size(log_weights),
               0.00000001);
-  ASSERT_NEAR(2.631578947, std::exp(filter.log_effective_samples(log_weights)),
+  ASSERT_NEAR(2.631578947, std::exp(filter.log_effective_sample_size(log_weights)),
               0.00000001);
 }
 
diff --git a/test/particle_filter_test.cpp b/test/particle_filter_test.cpp
index a1f55d66aea5a5eed5a7fc1938807bac07ab9e37..fd1dcf007ebb97759613c20b6fe3c28d969dfc9c 100644
--- a/test/particle_filter_test.cpp
+++ b/test/particle_filter_test.cpp
@@ -117,9 +117,9 @@ TEST(ParticleFilterTest, TestEffectiveSamplesSize)
 {
   auto filter = create_filter();
   std::vector<double> weights = {1. / 3, 1. / 3, 1. / 3};
-  ASSERT_DOUBLE_EQ(3., filter.effective_samples(weights));
+  ASSERT_DOUBLE_EQ(3., filter.effective_sample_size(weights));
   weights = {0.2, 0.3, 0.5};
-  ASSERT_NEAR(2.631578947, filter.effective_samples(weights), 0.00000001);
+  ASSERT_NEAR(2.631578947, filter.effective_sample_size(weights), 0.00000001);
 }
 
 int main(int argc, char **argv)
diff --git a/test/particle_replacer_test.cpp b/test/particle_replacer_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..691a4d08d97fc286e7be7d4c682e115d17b44e17
--- /dev/null
+++ b/test/particle_replacer_test.cpp
@@ -0,0 +1,86 @@
+#include <gtest/gtest.h>
+#include <filter_bay/utility/particle_replacer.hpp>
+using namespace filter_bay;
+
+const int SEED = 42;
+ParticleReplacer replacer(SEED);
+const size_t N_SAMPLES = 50000;
+const std::vector<int> OLD_STATES(N_SAMPLES, 0);
+const std::vector<int> NEW_STATES(N_SAMPLES, 1);
+const double ABS_ERROR = 0.1;
+
+void test_ratio(const std::vector<int>& states, double ratio) {
+  double mean = 0;
+  for (const auto& state : states) {
+    mean += (double)state / N_SAMPLES;
+  }
+  ASSERT_NEAR(mean, ratio, ABS_ERROR);
+}
+
+void test_ratio(double ratio) {
+  double mean = 0;
+  for (size_t i = 0; i < N_SAMPLES; i++) {
+    mean += (double)replacer.select(0, 1, ratio) / N_SAMPLES;
+  }
+  ASSERT_NEAR(mean, ratio, ABS_ERROR);
+}
+
+TEST(ParticleReplacerTest, TestReplaceVector) {
+  // test different ratios
+  double ratio = 0;
+  auto replaced_states = replacer.replace(OLD_STATES, NEW_STATES, ratio);
+  test_ratio(replaced_states, ratio);
+  ratio = 0.1;
+  replaced_states = replacer.replace(OLD_STATES, NEW_STATES, ratio);
+  test_ratio(replaced_states, ratio);
+  ratio = 0.3;
+  replaced_states = replacer.replace(OLD_STATES, NEW_STATES, ratio);
+  test_ratio(replaced_states, ratio);
+  ratio = 0.5;
+  replaced_states = replacer.replace(OLD_STATES, NEW_STATES, ratio);
+  test_ratio(replaced_states, ratio);
+  ratio = 0.9;
+  replaced_states = replacer.replace(OLD_STATES, NEW_STATES, ratio);
+  test_ratio(replaced_states, ratio);
+  ratio = 1;
+  replaced_states = replacer.replace(OLD_STATES, NEW_STATES, ratio);
+  test_ratio(replaced_states, ratio);
+}
+
+TEST(ParticleReplacerTest, TestReplaceState) {
+  // test different ratios
+  double ratio = 0;
+  auto replaced_states = replacer.replace(OLD_STATES, 1, ratio);
+  test_ratio(replaced_states, ratio);
+  ratio = 0.1;
+  replaced_states = replacer.replace(OLD_STATES, 1, ratio);
+  test_ratio(replaced_states, ratio);
+  ratio = 0.3;
+  replaced_states = replacer.replace(OLD_STATES, 1, ratio);
+  test_ratio(replaced_states, ratio);
+  ratio = 0.5;
+  replaced_states = replacer.replace(OLD_STATES, 1, ratio);
+  test_ratio(replaced_states, ratio);
+  ratio = 0.9;
+  replaced_states = replacer.replace(OLD_STATES, 1, ratio);
+  test_ratio(replaced_states, ratio);
+  ratio = 1;
+  replaced_states = replacer.replace(OLD_STATES, 1, ratio);
+  test_ratio(replaced_states, ratio);
+}
+
+TEST(ParticleReplacerTest, TestSelect) {
+  // test different ratios
+  double ratio = 0;
+  test_ratio(ratio);
+  ratio = 0.1;
+  test_ratio(ratio);
+  ratio = 0.3;
+  test_ratio(ratio);
+  ratio = 0.5;
+  test_ratio(ratio);
+  ratio = 0.9;
+  test_ratio(ratio);
+  ratio = 1;
+  test_ratio(ratio);
+}