diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 335e51fb3c16cd8fc083ceb5e2794eb1ee034343..3d3536f6c7eefa65e3a0f8176719c9de2727800f 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -251,6 +251,7 @@ SET(HDRS
     landmark_line_2D.h
     landmark_polyline_2D.h
     local_parametrization_polyline_extreme.h
+    processor_frame_nearest_neighbor_filter.h
     processor_image_params.h
     processor_imu.h
     processor_imu_UnitTester.h
@@ -321,6 +322,7 @@ SET(SRCS
     landmark_line_2D.cpp
     landmark_polyline_2D.cpp
     local_parametrization_polyline_extreme.cpp
+    processor_frame_nearest_neighbor_filter.cpp
     processor_imu.cpp
     processor_imu_UnitTester.cpp
     processor_odom_2D.cpp
diff --git a/src/processor_frame_nearest_neighbor_filter.cpp b/src/processor_frame_nearest_neighbor_filter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a9c1205653bd6f19c55c85c49058fdb75be22b9d
--- /dev/null
+++ b/src/processor_frame_nearest_neighbor_filter.cpp
@@ -0,0 +1,493 @@
+#include "processor_frame_nearest_neighbor_filter.h"
+
+namespace wolf
+{
+
+ProcessorFrameNearestNeighborFilter::ProcessorFrameNearestNeighborFilter(const Params& _params):
+    ProcessorLoopClosureBase("FRAME NEAREST NEIGHBOR FILTER"),
+    params_(_params)
+{
+  // area of ellipse based on the Chi-Square Probabilities
+  // https://people.richland.edu/james/lecture/m170/tbl-chi.html
+  // cover both 2D and 3D cases
+
+  if(params_.distance_type_ == DistanceType::LC_POINT_ELLIPSE ||
+     params_.distance_type_ == DistanceType::LC_ELLIPSE_ELLIPSE)
+  {
+    switch ((int)(params_.probability_*100))
+    {
+    case 900:  area_ = 4.605; // 90% probability
+      break;
+    case 950:  area_ = 5.991; // 95% probability
+      break;
+    case 990:  area_ = 9.210; // 99% probability
+      break;
+    default :  area_ = 5.991; // 95% probability
+      break;
+    }
+  }
+  if (params_.distance_type_ == DistanceType::LC_POINT_ELLIPSOID ||
+      params_.distance_type_ == DistanceType::LC_ELLIPSOID_ELLIPSOID)
+  {
+    switch ((int)(params_.probability_*100))
+    {
+    case 900:  area_ = 6.251;  // 90% probability
+      break;
+    case 950:  area_ = 7.815;  // 95% probability
+      break;
+    case 990:  area_ = 11.345; // 99% probability
+      break;
+    default :  area_ = 7.815;  // 95% probability
+      break;
+    }
+  }
+
+  /*
+   * The area is based on the Chi-Square Probabilities
+   * Becasue they are very big for high likelihood, it is here manually set
+   * on ONE. Therefore the ellipses/ ellipsoids are based on the unit ellipse/
+   * ellipsoids and the axis are scaled by a / b / c
+   */
+
+  area_ = 1;
+}
+
+//##############################################################################
+bool ProcessorFrameNearestNeighborFilter::findCandidates(const CaptureBasePtr& /*_incoming_ptr*/)
+{
+  const ProblemPtr problem_ptr = getProblem();
+
+  const std::string frame_structure =
+      problem_ptr->getTrajectoryPtr()->getFrameStructure();
+
+  // get the list of all frames
+  const FrameBaseList& frame_list = problem_ptr->
+                                    getTrajectoryPtr()->
+                                    getFrameList();
+
+  bool found_possible_candidate = false;
+
+  for (const FrameBasePtr& key_it : frame_list)
+  {
+    // check for LC just if frame is key frame
+    // Assert that the evaluated KF has a capture of the
+    // same sensor as this processor
+    if (key_it->isKey() && key_it->getCaptureOf(getSensorPtr()/*, "LASER 2D"*/) != nullptr)
+    {
+      // Check if the two frames currently evaluated are already
+      // constrained one-another.
+      const ConstraintBaseList& ctr_list = key_it->getConstrainedByList();
+
+      bool are_constrained = false;
+      for (const ConstraintBasePtr& crt : ctr_list)
+      {
+        // Are the two frames constrained one-another ?
+        if (crt->getFrameOtherPtr() == problem_ptr->getLastKeyFramePtr())
+        {
+          // By this very processor ?
+          if (crt->getProcessor() == shared_from_this())
+          {
+            WOLF_DEBUG("Frames are already constrained together.");
+            are_constrained = true;
+            break;
+          }
+        }
+      }
+      if (are_constrained) continue;
+
+      // check if feame is potential candidate for loop closure with
+      // chosen distance type
+      switch (params_.distance_type_)
+      {
+      case LoopclosureDistanceType::LC_POINT_ELLIPSE:
+      {
+        if (frame_structure == "PO 3D" || frame_structure == "POV 3D") // @todo add frame structure "PQVBB 3D"
+        {
+          WOLF_ERROR("Distance Type LC_POINT_ELLIPSE does not fit 3D frame structure");
+          return false;
+        }
+
+        Eigen::Vector5s frame_covariance;
+        if (!computeEllipse2D(key_it, frame_covariance)) continue;
+
+        const Eigen::VectorXs current_position = getProblem()->getCurrentState();
+        found_possible_candidate = point2DIntersect(current_position,
+                                                    frame_covariance);
+        break;
+      }
+
+      case LoopclosureDistanceType::LC_ELLIPSE_ELLIPSE:
+      {
+        if (frame_structure == "PO 3D" || frame_structure == "POV 3D")
+        {
+          WOLF_ERROR("Distance Type LC_ELLIPSE_ELLIPSE does not fit 3D frame structure");
+          return false;
+        }
+
+        Eigen::Vector5s frame_covariance, current_covariance;
+        if (!computeEllipse2D(key_it,
+                              frame_covariance)) continue;
+        if (!computeEllipse2D(getProblem()->getLastKeyFramePtr(),
+                              current_covariance)) continue;
+        found_possible_candidate = ellipse2DIntersect(frame_covariance,
+                                                      current_covariance);
+        break;
+      }
+
+      case LoopclosureDistanceType::LC_POINT_ELLIPSOID:
+      {
+        if (frame_structure == "PO 2D")
+        {
+          WOLF_ERROR("Distance Type POINT_ELLIPSOID does not fit 2D frame structure");
+          return false;
+        }
+        Eigen::Vector10s frame_covariance;
+        if (!computeEllipsoid3D(key_it,
+                                frame_covariance)) continue;
+        const Eigen::VectorXs current_position = getProblem()->getCurrentState();
+        found_possible_candidate = point3DIntersect(current_position,
+                                                    frame_covariance);
+        break;
+      }
+
+      case LoopclosureDistanceType::LC_ELLIPSOID_ELLIPSOID:
+      {
+        if (frame_structure == "PO 2D")
+        {
+          WOLF_ERROR("Distance Type ELLIPSOID_ELLIPSOID does not fit 2D frame structure");
+          return false;
+        }
+        Eigen::Vector10s frame_covariance, current_covariance;
+        if (!computeEllipsoid3D(key_it,
+                                frame_covariance)) continue;
+        if (!computeEllipsoid3D(getProblem()->getLastKeyFramePtr(),
+                                frame_covariance)) continue;
+        found_possible_candidate = ellipsoid3DIntersect(frame_covariance,
+                                                        current_covariance);
+        break;
+      }
+
+      case LoopclosureDistanceType::LC_MAHALANOBIS_DISTANCE:
+      {
+        found_possible_candidate = insideMahalanisDistance(key_it,
+                                             problem_ptr->getLastKeyFramePtr());
+        break;
+      }
+
+      default:
+        WOLF_WARN("NO IMPLEMENTATION FOR OTHER DISTANCE CALCULATION TYPES YET.");
+        found_possible_candidate = false;
+        return false;
+      }
+
+      // if a candidate was detected, push it to the appropiate list
+      if (found_possible_candidate)
+      {
+        if (!frameInsideBuffer(key_it))
+          loop_closure_candidates.push_back(key_it);
+        else
+          close_candidates.push_back(key_it);
+      }
+
+    } // end if key_it is keyframe
+  } // end loop through every frame in list
+
+  return found_possible_candidate;
+}
+
+//##############################################################################
+bool ProcessorFrameNearestNeighborFilter::computeEllipse2D(const FrameBasePtr& frame_ptr,
+                                                           Eigen::Vector5s& ellipse)
+{
+  // get 3x3 covariance matrix AKA variance
+  const Eigen::MatrixXs covariance =
+      getProblem()->getFrameCovariance(frame_ptr);
+
+  if (!isCovariance(covariance))
+  {
+    WOLF_WARN("Covariance is not proper !");
+    return false;
+  }
+
+  // get position of frame AKA mean [x, y]
+  const Eigen::VectorXs frame_position  = frame_ptr->getPPtr()->getState();
+  ellipse(0) = frame_position(0);
+  ellipse(1) = frame_position(1);
+
+  // compute covariance error ellipse around state
+  Eigen::SelfAdjointEigenSolver<Eigen::Matrix2s> solver(covariance.block(0,0,2,2));
+  Scalar eigenvalue1 = solver.eigenvalues()[0];     // smaller value
+  Scalar eigenvalue2 = solver.eigenvalues()[1];     // bigger value
+  //Eigen::VectorXs eigenvector1 = solver.eigenvectors().col(0);
+  Eigen::VectorXs eigenvector2 = solver.eigenvectors().col(1);
+
+  const Scalar scale = std::sqrt(area_);
+  ellipse(2) = scale * std::sqrt(eigenvalue2) / 2.;          // majorAxis
+  ellipse(3) = scale * std::sqrt(eigenvalue1) / 2.;          // minorAxis
+  ellipse(4) = std::atan2(eigenvector2[1], eigenvector2[0]);  // tilt
+
+  if (ellipse(4) < Scalar(0)) ellipse(4) += Scalar(2) * M_PI;
+
+  // [ pos_x, pos_y, a, b, tilt]
+  return true;
+}
+
+//##############################################################################
+bool ProcessorFrameNearestNeighborFilter::computeEllipsoid3D(const FrameBasePtr& frame_pointer,
+                                                             Eigen::Vector10s& ellipsoid)
+{
+  // Ellipse description [ pos_x, pos_y, pos_z, a, b, c, quat_w, quat_z, quat_y, quat_z]
+
+  // get position of frame AKA mean [x, y, z]
+  const Eigen::VectorXs frame_position  = frame_pointer->getPPtr()->getState();
+  ellipsoid(0) = frame_position(0);
+  ellipsoid(1) = frame_position(1);
+  ellipsoid(2) = frame_position(2);
+
+  // get 9x9 covariance matrix AKA variance
+  const Eigen::MatrixXs covariance = getProblem()->getFrameCovariance(frame_pointer);
+
+  if (!isCovariance(covariance)) return false;
+
+  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3s> solver(covariance.block(0,0,3,3));
+  const Scalar eigenvalue1 = solver.eigenvalues()[0];     // smaller value
+  const Scalar eigenvalue2 = solver.eigenvalues()[1];     // mediate value
+  const Scalar eigenvalue3 = solver.eigenvalues()[2];     // bigger value
+
+  const Scalar scale = std::sqrt(area_);
+
+  ellipsoid(3) = scale * std::sqrt(std::abs(eigenvalue3)) / Scalar(2); // majorAxis
+  ellipsoid(4) = scale * std::sqrt(std::abs(eigenvalue2)) / Scalar(2); // mediumAxis
+  ellipsoid(5) = scale * std::sqrt(std::abs(eigenvalue1)) / Scalar(2); // minorAxis
+
+  // ROTATION COMPUTATION get rotation of the three axis from eigenvector
+  // eigenvector belonging to biggest eigenvalue gives direction / z_axis
+  // of ellipsoid
+
+  // get eigenvectors that correspond to eigenvalues
+  //const Eigen::Vector3s eigenvector1 = solver.eigenvectors().col(0); // minorAxis
+  const Eigen::Vector3s eigenvector2 = solver.eigenvectors().col(1); // mediumAxis ->y
+  const Eigen::Vector3s eigenvector3 = solver.eigenvectors().col(2); // majorAxis -> x
+
+  // computed axis of coordinate system in ellipsoid center
+  const Eigen::Vector3s z_axis = eigenvector3.cross(eigenvector2);
+  const Eigen::Vector3s y_axis = z_axis.cross(eigenvector3);
+
+  // use them to fill rotation matrix
+  Eigen::Matrix3s rotationMatrix;
+  rotationMatrix.col(0) = eigenvector3.normalized();
+  rotationMatrix.col(1) = y_axis.normalized();
+  rotationMatrix.col(2) = z_axis.normalized();
+
+  // get quaternions for transformation
+  Eigen::Quaternions rotation(rotationMatrix);
+  rotation.normalize();
+
+  ellipsoid(6) = rotation.w();
+  ellipsoid(7) = rotation.x();
+  ellipsoid(8) = rotation.y();
+  ellipsoid(9) = rotation.z();
+
+  WOLF_DEBUG("made ellipsoid: \n[", ellipsoid.transpose(), "]");
+
+  // [ pos_x, pos_y, pos_z, a, b, c, alpha, beta, gamma]
+  return true;
+}
+
+//##############################################################################
+bool ProcessorFrameNearestNeighborFilter::ellipse2DIntersect(const Eigen::VectorXs& ellipse1,
+                                                             const Eigen::VectorXs& ellipse2)
+{
+  for (int i = 0 ; i < 360 ; i += params_.sample_step_degree_)
+  {
+    // parameterized form of first ellipse gives point on first ellipse
+    // https://www.uwgb.edu/dutchs/Geometry/HTMLCanvas/ObliqueEllipses5.HTM
+    Eigen::VectorXs pointOnEllipse(2);
+    const Scalar theta = Scalar(i) * M_PI / 180.0;
+
+    pointOnEllipse(0) = ellipse1(0) +
+                          ellipse1(2) * std::cos(ellipse1(4)) * std::cos(theta) -
+                          ellipse1(3) * std::sin(ellipse1(4)) * std::sin(theta);
+
+    pointOnEllipse(1) = ellipse1(1) +
+                          ellipse1(2) * std::sin(ellipse1(4)) * std::cos(theta) +
+                          ellipse1(3) * std::cos(ellipse1(4)) * std::sin(theta);
+
+    //WOLF_DEBUG(" for ", i, " deg / ", theta " rad --->   [" ,
+    //            pointOnEllipse.transpose(), "]");
+
+    // check if point lies inside the other ellipse
+    if (point2DIntersect(pointOnEllipse, ellipse2)) return true;
+  }
+
+  return false;
+}
+
+//##############################################################################
+bool ProcessorFrameNearestNeighborFilter::point2DIntersect(const Eigen::VectorXs& point,
+                                                           const Eigen::VectorXs& ellipse)
+{
+  const Scalar area51 = std::pow( ((point(0) - ellipse(0)) * cos(ellipse(4))
+                                  + (point(1) - ellipse(1)) * sin(ellipse(4))), 2 )
+                        / std::pow( ellipse(2), 2 )
+                        + std::pow( ((point(0) - ellipse(0)) * sin(ellipse(4))
+                                    - (point(1) - ellipse(1)) * cos(ellipse(4))), 2 )
+                        / std::pow( ellipse(3), 2 );
+
+  //WOLF_DEBUG("computed area = ", area51);
+
+  if ( area51 - area_ <=  0) return true;
+
+  return false;
+}
+
+//##############################################################################
+bool ProcessorFrameNearestNeighborFilter::ellipsoid3DIntersect(const Eigen::VectorXs &ellipsoid1,
+                                                               const Eigen::VectorXs &ellipsoid2)
+{
+  // get transformation from elliposiod1 center frame to world frame
+  Eigen::Matrix4s transformation_matrix;
+  Eigen::Quaternions rotation(ellipsoid1(6),ellipsoid1(7),
+                              ellipsoid1(8),ellipsoid1(9));
+//  Eigen::Vector4s translation;
+//  translation << ellipsoid1(0), ellipsoid1(1), ellipsoid1(2), 1;
+  transformation_matrix.block(0,0,3,3) = rotation.toRotationMatrix();
+  transformation_matrix.col(3) << ellipsoid1(0), ellipsoid1(1), ellipsoid1(2), 1;
+
+  Eigen::Vector4s point_hom = Eigen::Vector4s::Constant(1);
+
+  Scalar omega = 0;
+  for (int i = 0 ; i < 360 ; i += params_.sample_step_degree_)
+  {
+    const Scalar theta = Scalar(i) * M_PI / 180.0;
+    for( int j = 0 ; j < 180 ; j += params_.sample_step_degree_)
+    {
+      omega = Scalar(j) * M_PI / 180.0;
+
+      // compute point on surface of first ellipsoid
+      point_hom(0) = ellipsoid1(3) * std::cos(theta) * std::sin(omega);
+      point_hom(1) = ellipsoid1(4) * std::sin(theta) * std::sin(omega);
+      point_hom(2) = ellipsoid1(5) * std::cos(omega);
+
+      // transform point into world frame
+      const Eigen::Vector4s point_on_ellipsoid = transformation_matrix * point_hom;
+
+      // check if 3D point lies inside the second ellipsoid
+      if (point3DIntersect(point_on_ellipsoid, ellipsoid2))
+        return true;
+    }
+  }
+  return false;
+}
+
+//##############################################################################
+bool ProcessorFrameNearestNeighborFilter::point3DIntersect(const Eigen::VectorXs &point,
+                                                           const Eigen::VectorXs &ellipsoid)
+{
+  // get transformation from ellipsoid center frame to world frame
+  Eigen::Matrix4s transformation_matrix = Eigen::Matrix4s::Identity();
+
+  Eigen::Quaternions rotation(ellipsoid(6),ellipsoid(7),
+                              ellipsoid(8),ellipsoid(9));
+//  Eigen::Vector4s translation;
+//  translation << ellipsoid(0), ellipsoid(1), ellipsoid(2), 1;
+  transformation_matrix.block(0,0,3,3) = rotation.toRotationMatrix();
+  transformation_matrix.col(3) << ellipsoid(0), ellipsoid(1), ellipsoid(2), 1;
+  // inverse to get transformation from world frame to ellipsoid center frame
+  transformation_matrix = transformation_matrix.inverse().eval();
+
+  // homogenize 3D point
+  // ???
+  Eigen::Vector4s point_hom;
+  point_hom << point(0), point(1), point(2), 1;
+
+  // transform point from world frame to elliposiod center frame
+  Eigen::Vector4s transformed_point = transformation_matrix * point_hom;
+
+  // check if point is inside ellipsoid with general equation
+  Scalar area51 = std::pow(transformed_point(0), 2) / std::pow( ellipsoid(3), 2) +
+      std::pow(transformed_point(1), 2) / std::pow(ellipsoid(4), 2) +
+      std::pow(transformed_point(2), 2) / std::pow(ellipsoid(5), 2);
+
+  WOLF_DEBUG("computed area = ", area51);
+
+  if (area51 - area_ <=  0) return true;
+
+  return false;
+}
+
+//##############################################################################
+bool ProcessorFrameNearestNeighborFilter::insideMahalanisDistance(const FrameBasePtr& trajectory_frame,
+                                                                  const FrameBasePtr& query_frame)
+{
+  const Scalar distance = MahalanobisDistance(trajectory_frame, query_frame);
+
+  WOLF_DEBUG("Mahalanobis Distance = ", distance);
+
+  if ( distance < 0 )
+  {
+    WOLF_DEBUG("NO COVARIANCE AVAILABLE");
+  }
+
+  /*
+     * TODO:
+     * check if distance is smaller than a threshold,
+     * if yes -> return true
+     * else -> return false
+     *
+     */
+  return false;
+}
+
+//##############################################################################
+Scalar ProcessorFrameNearestNeighborFilter::MahalanobisDistance(const FrameBasePtr& trajectory,
+                                                                const FrameBasePtr& query)
+{
+  Scalar distance = -1;
+  Eigen::VectorXs traj_pose, query_pose;
+
+  // get state and covariance matrix for both frames
+  if (trajectory->getPPtr()->getState().size() == 2)
+  {
+    traj_pose.resize(3);
+    query_pose.resize(3);
+  }
+  else
+  {
+    traj_pose.resize(7);
+    query_pose.resize(7);
+  }
+
+  traj_pose << trajectory->getPPtr()->getState(),
+               trajectory->getOPtr()->getState();
+
+  query_pose << query->getPPtr()->getState(),
+                query->getOPtr()->getState();
+
+  const Eigen::MatrixXs traj_covariance  = getProblem()->getFrameCovariance(trajectory);
+  const Eigen::MatrixXs query_covariance = getProblem()->getFrameCovariance(query);
+
+  if ( !isCovariance(traj_covariance) || !isCovariance(query_covariance))
+    return distance;
+
+  const Eigen::MatrixXs covariance = traj_covariance * query_covariance.transpose();
+
+  const Eigen::VectorXs pose_difference = traj_pose - query_pose;
+  distance = pose_difference.transpose() * covariance * pose_difference;
+  distance = std::sqrt(distance);
+
+  return distance;
+}
+
+//##############################################################################
+bool ProcessorFrameNearestNeighborFilter::frameInsideBuffer(const FrameBasePtr& frame_ptr)
+{
+  FrameBasePtr keyframe = getProblem()->getLastKeyFramePtr();
+  if ( (int)frame_ptr->id() < ( (int)keyframe->id() - params_.buffer_size_ ))
+    return false;
+  else
+    return true;
+}
+
+} // namespace wolf
+
diff --git a/src/processor_frame_nearest_neighbor_filter.h b/src/processor_frame_nearest_neighbor_filter.h
new file mode 100644
index 0000000000000000000000000000000000000000..16c3985e035ce795e116fadec3e2ae1a83e224da
--- /dev/null
+++ b/src/processor_frame_nearest_neighbor_filter.h
@@ -0,0 +1,114 @@
+#ifndef _WOLF_SRC_PROCESSOR_FRAME_NEAREST_NEIGHBOR_FILTER_H
+#define _WOLF_SRC_PROCESSOR_FRAME_NEAREST_NEIGHBOR_FILTER_H
+
+// Wolf related headers
+#include "processor_loopclosure_base.h"
+#include "state_block.h"
+
+namespace wolf{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsFrameNearestNeighborFilter)
+WOLF_PTR_TYPEDEFS(ProcessorFrameNearestNeighborFilter)
+
+enum class LoopclosureDistanceType : std::size_t
+{
+  LC_POINT_ELLIPSE = 1,       // 2D
+  LC_ELLIPSE_ELLIPSE,         // 2D
+  LC_POINT_ELLIPSOID,         // 3D
+  LC_ELLIPSOID_ELLIPSOID,     // 3D
+  LC_MAHALANOBIS_DISTANCE     // 2D & 3D
+};
+
+struct ProcessorParamsFrameNearestNeighborFilter : public ProcessorParamsLoopClosure
+{
+  using DistanceType = LoopclosureDistanceType;
+
+  ProcessorParamsFrameNearestNeighborFilter() :
+    buffer_size_(10),
+    sample_step_degree_(10),
+    distance_type_(LoopclosureDistanceType::LC_POINT_ELLIPSE),
+    probability_(5.991)  { }
+
+  ProcessorParamsFrameNearestNeighborFilter(
+      const ProcessorParamsFrameNearestNeighborFilter& o) :
+    buffer_size_(o.buffer_size_),
+    sample_step_degree_(o.sample_step_degree_),
+    distance_type_(o.distance_type_),
+    probability_(o.probability_)
+  {
+    //
+  }
+
+  virtual ~ProcessorParamsFrameNearestNeighborFilter() = default;
+
+  int buffer_size_;
+  int sample_step_degree_;
+  DistanceType distance_type_;
+  Scalar probability_;
+};
+
+class ProcessorFrameNearestNeighborFilter : public ProcessorLoopClosureBase
+{
+private:
+
+  // area of the computed covariance ellipse.
+  // depends on how many percent of data should be considered.
+  wolf::Scalar area_;
+
+  ProcessorParamsFrameNearestNeighborFilter params_;
+
+public:
+
+  using Params    = ProcessorParamsFrameNearestNeighborFilter;
+  using ParamsPtr = ProcessorParamsFrameNearestNeighborFilterPtr;
+  using DistanceType = Params::DistanceType;
+
+  ProcessorFrameNearestNeighborFilter(const Params& _params);
+
+  virtual ~ProcessorFrameNearestNeighborFilter() = default;
+
+  inline DistanceType getDistanceType() const noexcept {return params_.distance_type_;}
+
+protected:
+
+  virtual bool findCandidates(const CaptureBasePtr& _incoming_ptr);
+
+  // returns Ellipse in 2D case [ pos_x, pos_y, a, b, tilt]
+  bool computeEllipse2D(const FrameBasePtr& frame_ptr,
+                        Eigen::Vector5s& ellipse);
+
+  // returns Ellipsoid in 3D case
+  // [ pos_x, pos_y, pos_z, a, b, c, quat_w, quat_z, quat_y, quat_z]
+  bool computeEllipsoid3D(const FrameBasePtr& frame_ptr,
+                          Eigen::Vector10s& ellipsoid);
+
+  // returns true if the two 2D ellipses intersect
+  bool ellipse2DIntersect(const Eigen::VectorXs &ellipse1,
+                          const Eigen::VectorXs &ellipse2);
+
+  // returns true if a 2D point lies inside a 2D ellipse
+  bool point2DIntersect(const Eigen::VectorXs &point,
+                        const Eigen::VectorXs &ellipse);
+
+  // returns true if the two 3D ellipsoids intersect
+  bool ellipsoid3DIntersect(const Eigen::VectorXs &ellipsoid1,
+                            const Eigen::VectorXs &ellipsoid2);
+
+  // returns true if a 3D point lies inside a 3D ellipsoid
+  bool point3DIntersect(const Eigen::VectorXs &point,
+                        const Eigen::VectorXs &ellipsoid);
+
+  // returns true if frame lies within Mahalanobis Distance
+  bool insideMahalanisDistance(const FrameBasePtr& trajectory_frame,
+                               const FrameBasePtr& query_frame);
+
+  // computes the Mahalanobis Distance
+  Scalar MahalanobisDistance(const FrameBasePtr& trajectory_frame,
+                             const FrameBasePtr& query_frame);
+
+  bool frameInsideBuffer(const FrameBasePtr& frame_ptr);
+};
+
+} // namespace wolf
+
+#endif // _WOLF_SRC_PROCESSOR_FRAME_NEAREST_NEIGHBOR_FILTER_H_
diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt
index 3763631a09f9297e2c7758383fe92d93ef9aaf33..8bd7628d1bf8b7fc4964d53e143b0672c6b5396b 100644
--- a/src/test/CMakeLists.txt
+++ b/src/test/CMakeLists.txt
@@ -114,6 +114,10 @@ target_link_libraries(gtest_frame_imu ${PROJECT_NAME})
 wolf_add_gtest(gtest_pinhole gtest_pinhole.cpp)
 target_link_libraries(gtest_pinhole ${PROJECT_NAME})
 
+# ProcessorFrameNearestNeighborFilter class test
+wolf_add_gtest(gtest_processor_frame_nearest_neighbor_filter_2D gtest_processor_frame_nearest_neighbor_filter_2D.cpp)
+target_link_libraries(gtest_processor_frame_nearest_neighbor_filter_2D ${PROJECT_NAME})
+
 # ProcessorIMU class test
 wolf_add_gtest(gtest_processor_imu gtest_processor_imu.cpp)
 target_link_libraries(gtest_processor_imu ${PROJECT_NAME})
@@ -146,4 +150,4 @@ IF(OpenCV_FOUND)
 
   wolf_add_gtest(gtest_roi_ORB gtest_roi_ORB.cpp)
   target_link_libraries(gtest_roi_ORB ${PROJECT_NAME})
-ENDIF(OpenCV_FOUND)
\ No newline at end of file
+ENDIF(OpenCV_FOUND)
diff --git a/src/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp b/src/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..47c79e0f137b1e2911575b0c74b640fb03e3c896
--- /dev/null
+++ b/src/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
@@ -0,0 +1,236 @@
+
+/**
+ * \file gtest_processor_frame_nearest_neighbor_filter.cpp
+ *
+ *  Created on: Aug 2, 2017
+ *      \author: tessajohanna
+ */
+
+
+#include "utils_gtest.h"
+#include "../logging.h"
+
+#include "../sensor_odom_2D.h"
+#include "../processor_frame_nearest_neighbor_filter.h"
+
+#include <iostream>
+
+// Dummy class so that it is no pur virtual
+struct DummyLoopCloser : public wolf::ProcessorFrameNearestNeighborFilter
+{
+  DummyLoopCloser(const Params& _params) :
+    wolf::ProcessorFrameNearestNeighborFilter(_params) { }
+
+  bool confirmLoopClosure() override { return false; }
+  bool voteForKeyFrame()    override { return false; }
+};
+
+// Declare Wolf problem
+wolf::ProblemPtr problem = wolf::Problem::create("PO 2D");
+
+// Declare Sensor
+Eigen::Vector3s odom_extrinsics = Eigen::Vector3s(0,0,0);
+
+std::shared_ptr<wolf::IntrinsicsOdom2D> odom_intrinsics =
+             std::make_shared<wolf::IntrinsicsOdom2D>(wolf::IntrinsicsOdom2D());
+
+wolf::SensorBasePtr sensor_ptr;
+
+// Declare Processors
+wolf::ProcessorFrameNearestNeighborFilterPtr processor_ptr_point2d;
+wolf::ProcessorParamsFrameNearestNeighborFilterPtr processor_params_point2d =
+                      std::make_shared<wolf::ProcessorParamsFrameNearestNeighborFilter>();
+
+wolf::ProcessorFrameNearestNeighborFilterPtr processor_ptr_ellipse2d;
+wolf::ProcessorParamsFrameNearestNeighborFilterPtr processor_params_ellipse2d =
+                      std::make_shared<wolf::ProcessorParamsFrameNearestNeighborFilter>();
+
+// Declare Frame Pointer
+wolf::StateBlockPtr stateblock_ptr1, stateblock_ptr2, stateblock_ptr3, stateblock_ptr4;
+wolf::FrameBasePtr F1, F2, F3, F4;
+wolf::CaptureBasePtr capture_dummy, incomming_dummy;
+
+//##############################################################################
+TEST(ProcessorFrameNearestNeighborFilter, PointInEllipseRotated)
+{
+  // four different states           [x, y]
+  Eigen::Vector3s state1, state2, state3, state4;
+  state1 << 3.0, 5.0, 0.0;
+  state2 << 3.0, 6.0, 0.0;
+  state3 << 3.0, 7.0, 0.0;
+  state4 << 100.0, 100.0, 0.0;
+
+  auto stateblock_pptr1 = std::make_shared<wolf::StateBlock>(state1.head<2>());
+  auto stateblock_optr1 = std::make_shared<wolf::StateBlock>(state1.tail<1>());
+
+  auto stateblock_pptr2 = std::make_shared<wolf::StateBlock>(state2.head<2>());
+  auto stateblock_optr2 = std::make_shared<wolf::StateBlock>(state2.tail<1>());
+
+  auto stateblock_pptr3 = std::make_shared<wolf::StateBlock>(state3.head<2>());
+  auto stateblock_optr3 = std::make_shared<wolf::StateBlock>(state3.tail<1>());
+
+  auto stateblock_pptr4 = std::make_shared<wolf::StateBlock>(state4.head<2>());
+  auto stateblock_optr4 = std::make_shared<wolf::StateBlock>(state4.tail<1>());
+
+  // create Keyframes
+  F1 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME,
+                                         1,
+                                         stateblock_pptr1,
+                                         stateblock_optr1);
+  F2 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME,
+                                         1,
+                                         stateblock_pptr2,
+                                         stateblock_optr2);
+  F3 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME,
+                                         1,
+                                         stateblock_pptr3,
+                                         stateblock_optr3);
+  F4 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME,
+                                         1,
+                                         stateblock_pptr4,
+                                         stateblock_optr4);
+
+  // add dummy capture
+  capture_dummy = std::make_shared<wolf::CaptureBase>("DUMMY",
+                                                      1.0,
+                                                      sensor_ptr);
+  F1->addCapture(capture_dummy);
+  F2->addCapture(capture_dummy);
+  F3->addCapture(capture_dummy);
+  F4->addCapture(capture_dummy);
+
+  // Add first three states to trajectory
+  problem->getTrajectoryPtr()->addFrame(F1);
+  problem->getTrajectoryPtr()->addFrame(F2);
+  problem->getTrajectoryPtr()->addFrame(F3);
+  problem->getTrajectoryPtr()->addFrame(F4);
+
+  // Add same covariances for all states
+  Eigen::Matrix2s position_covariance_matrix;
+  position_covariance_matrix << 9.0, 0.0,
+                                0.0, 5.0;
+
+  Eigen::Matrix1s orientation_covariance_matrix;
+  orientation_covariance_matrix << 0.0;
+
+  Eigen::Vector2s tt_covariance_matrix;
+  tt_covariance_matrix << 0.0, 0.0;
+
+  problem->addCovarianceBlock( stateblock_pptr1, stateblock_pptr1,
+                               position_covariance_matrix);
+  problem->addCovarianceBlock( stateblock_optr1, stateblock_optr1,
+                               orientation_covariance_matrix);
+  problem->addCovarianceBlock( stateblock_pptr1, stateblock_optr1,
+                               tt_covariance_matrix);
+
+  problem->addCovarianceBlock( stateblock_pptr2, stateblock_pptr2,
+                               position_covariance_matrix);
+  problem->addCovarianceBlock( stateblock_optr2, stateblock_optr2,
+                               orientation_covariance_matrix);
+  problem->addCovarianceBlock( stateblock_pptr2, stateblock_optr2,
+                               tt_covariance_matrix);
+
+  problem->addCovarianceBlock( stateblock_pptr3, stateblock_pptr3,
+                               position_covariance_matrix);
+  problem->addCovarianceBlock( stateblock_optr3, stateblock_optr3,
+                               orientation_covariance_matrix);
+  problem->addCovarianceBlock( stateblock_pptr3, stateblock_optr3,
+                               tt_covariance_matrix);
+
+  problem->addCovarianceBlock( stateblock_pptr4, stateblock_pptr4,
+                               position_covariance_matrix);
+  problem->addCovarianceBlock( stateblock_optr4, stateblock_optr4,
+                               orientation_covariance_matrix);
+  problem->addCovarianceBlock( stateblock_pptr4, stateblock_optr4,
+                               tt_covariance_matrix);
+
+  // create dummy capture
+  incomming_dummy = std::make_shared<wolf::CaptureBase>("DUMMY",
+                                                        1.2,
+                                                        sensor_ptr);
+  // Make 3rd frame last Keyframe
+  problem->getTrajectoryPtr()->setLastKeyFramePtr(F3);
+
+  // trigger search for loopclosure
+  processor_ptr_point2d->process(incomming_dummy);
+
+  const std::vector<wolf::FrameBasePtr> &testloops =
+      processor_ptr_point2d->getCandidates();
+
+  ASSERT_EQ(testloops.size(), 1);
+  ASSERT_EQ(testloops[0]->id(), 2);
+}
+
+//##############################################################################
+TEST(ProcessorFrameNearestNeighborFilter, EllipseEllipseRotatedCrosscovariance)
+{
+  // set covariance to a -90 degree turned ellipse
+  Eigen::Matrix2s position_covariance_matrix;
+//  position_covariance_matrix << 9.0, 1.8,
+//                                1.8, 5.0;
+
+  position_covariance_matrix << 5.0, 0.0,
+                                0.0, 9.0;
+
+  problem->addCovarianceBlock( F1->getPPtr(), F1->getPPtr(),
+                               position_covariance_matrix);
+  problem->addCovarianceBlock( F2->getPPtr(), F2->getPPtr(),
+                               position_covariance_matrix);
+  problem->addCovarianceBlock( F3->getPPtr(), F3->getPPtr(),
+                               position_covariance_matrix);
+  problem->addCovarianceBlock( F4->getPPtr(), F4->getPPtr(),
+                               position_covariance_matrix);
+
+  // Make 3rd frame last Keyframe
+  problem->getTrajectoryPtr()->setLastKeyFramePtr(F3);
+
+  // trigger search for loopclosure
+  processor_ptr_ellipse2d->process(incomming_dummy);
+  const std::vector<wolf::FrameBasePtr> &testloops =
+      processor_ptr_ellipse2d->getCandidates();
+
+  ASSERT_EQ(testloops.size(), 2);
+  ASSERT_EQ(testloops[0]->id(), 1);
+  ASSERT_EQ(testloops[1]->id(), 2);
+
+  // Make 4th frame last Keyframe
+  problem->getTrajectoryPtr()->setLastKeyFramePtr(F4);
+
+  // trigger search for loopclosure again
+  processor_ptr_ellipse2d->process(incomming_dummy);
+  ASSERT_EQ(testloops.size(), 0);
+}
+
+//##############################################################################
+int main(int argc, char **argv)
+{
+  // SENSOR PARAMETERS
+  odom_intrinsics->k_disp_to_disp = 0.2;
+  odom_intrinsics->k_rot_to_rot = 0.2;
+
+  // install sensor
+  sensor_ptr = problem->installSensor("ODOM 2D", "odom",
+                                      odom_extrinsics, odom_intrinsics);
+
+  // install the processors
+  processor_params_point2d->probability_ = 0.95;
+  processor_params_point2d->buffer_size_ = 0;         // just exclude identical frameIDs
+  processor_params_point2d->distance_type_ = wolf::LoopclosureDistanceType::LC_POINT_ELLIPSE;
+
+  processor_ptr_point2d = std::make_shared<DummyLoopCloser>(*processor_params_point2d);
+  processor_ptr_point2d->setName("processor2Dpoint");
+
+  sensor_ptr->addProcessor(processor_ptr_point2d);
+
+  processor_params_ellipse2d->probability_ = 0.95;
+  processor_params_ellipse2d->buffer_size_ = 0;         // just exclude identical frameIDs
+  processor_params_ellipse2d->distance_type_ = wolf::LoopclosureDistanceType::LC_ELLIPSE_ELLIPSE;
+
+  processor_ptr_ellipse2d = std::make_shared<DummyLoopCloser>(*processor_params_ellipse2d);
+  processor_ptr_ellipse2d->setName("processor2Dellipse");
+
+  sensor_ptr->addProcessor(processor_ptr_ellipse2d);
+
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}