diff --git a/simcontroller/src/main.rs b/simcontroller/src/main.rs
index a2b5791..886c727 100644
--- a/simcontroller/src/main.rs
+++ b/simcontroller/src/main.rs
@@ -1,5 +1,5 @@
 use camera::Camera;
-use opencv::{core::{Ptr, VecN, CV_8UC4}, features2d::ORB, highgui, imgcodecs::ImreadModes, imgproc, prelude::Feature2DTrait};
+use opencv::{core::{KeyPoint, Mat, Ptr, VecN, Vector, CV_8UC4}, features2d::{draw_matches_def, BFMatcher, ORB}, highgui, imgcodecs::ImreadModes, imgproc, prelude::{DescriptorMatcherTrait, DescriptorMatcherTraitConst, Feature2DTrait}};
 use webots::prelude::*;
 use std::{f64::consts::TAU, time::Duration};
 mod camera;
@@ -13,6 +13,10 @@ struct MyRobot {
     right_motor: Motor,
     window: &'static str,
     orb_detector: Ptr<ORB>,
+    matcher: BFMatcher,
+    last_frame_descriptors: Option<Mat>,
+    last_frame_keypoints: Option<Vector<KeyPoint>>,
+    last_image: Option<Mat>,
 }
 
 impl Robot for MyRobot {
@@ -35,6 +39,8 @@ impl Robot for MyRobot {
         //highgui::named_window(window, 1).unwrap();
 
         let orb_detector = ORB::create_def().unwrap();
+        let matcher = opencv::features2d::BFMatcher::new_def().unwrap();
+        //let last_frame_descriptors = opencv::core::Mat::default();
 
         Self {
             camera,
@@ -42,6 +48,10 @@ impl Robot for MyRobot {
             right_motor,
             window,
             orb_detector,
+            matcher,
+            last_frame_descriptors: None,
+            last_frame_keypoints: None,
+            last_image: None,
         }
     }
 
@@ -62,8 +72,23 @@ impl Robot for MyRobot {
             self.orb_detector.detect_and_compute_def(&img, &mask, &mut keypoints, &mut descriptors).unwrap();
             opencv::features2d::draw_keypoints(&img, &keypoints, &mut debugout, opencv::core::VecN([0.,255.,255.,255.]), opencv::features2d::DrawMatchesFlags::DEFAULT).unwrap();
             highgui::imshow("keypoints", &debugout).unwrap();
-            highgui::poll_key().unwrap();
+
+
+            let mut matches: Vector<opencv::core::DMatch> = Vector::new();
+
+
+            if let Some(mut last_frame_descriptors) = self.last_frame_descriptors.as_ref() {
+                self.matcher.train_match_def(&mut last_frame_descriptors, &mut descriptors, &mut matches).unwrap();
+
+                let matches = matches.iter().filter(|m| m.distance < 105.).collect();
+                draw_matches_def(self.last_image.as_ref().unwrap(), self.last_frame_keypoints.as_ref().unwrap(), &img , &keypoints, &matches, &mut debugout).unwrap();
+                highgui::imshow("matches", &debugout).unwrap();
+            }
+            self.last_frame_descriptors = Some(descriptors);
+            self.last_frame_keypoints = Some(keypoints);
+            self.last_image = Some(img.clone_pointee());
         }
+        highgui::poll_key().unwrap();
 
         // initialize motor speeds at 50% of MAX_SPEED.
         let mut left_speed = 0.5 * MAX_SPEED;