diff --git a/sw/airborne/modules/computer_vision/lib/vision/image.c b/sw/airborne/modules/computer_vision/lib/vision/image.c
index 734d534c40..98a9dde41a 100644
--- a/sw/airborne/modules/computer_vision/lib/vision/image.c
+++ b/sw/airborne/modules/computer_vision/lib/vision/image.c
@@ -235,6 +235,108 @@ void image_yuv422_downsample(struct image_t *input, struct image_t *output, uint
   }
 }
 
+/**
+ * This function adds padding to input image by mirroring the edge image elements.
+ * @param[in]  *input  - input image (grayscale only)
+ * @param[out] *output - the output image
+ * @param[in]  border_size  - amount of padding around image. Padding is made by reflecting image elements at the edge
+ * 						      Example: f e d c b a | a b c d e f | f e d c b a
+ */
+void image_add_border(struct image_t *input, struct image_t *output, uint8_t border_size)
+{
+	image_create(output, input->w + 2 * border_size, input->h + 2 * border_size, input->type);
+
+	uint8_t *input_buf = (uint8_t *)input->buf;
+	uint8_t *output_buf = (uint8_t *)output->buf;
+
+	// Skip first `border_size` rows, iterate through next input->h rows
+	for (uint16_t i = border_size; i != (output->h - border_size); i++){
+
+		// Mirror first `border_size` columns
+		for (uint8_t j = 0; j != border_size; j++)
+			output_buf[i * output->w + (border_size - 1 - j)] = input_buf[(i - border_size) * input->w + j];
+
+		// Copy corresponding row values from input image
+		memcpy(&output_buf[i * output->w + border_size], &input_buf[(i - border_size) * input->w], sizeof(uint8_t) * input->w);
+
+		// Mirror last `border_size` columns
+		for (uint8_t j = 0; j != border_size; j++)
+			output_buf[i * output->w + output->w - border_size + j] = output_buf[i * output->w + output->w - border_size -1 - j];
+	}
+
+	// Mirror first `border_size` and last `border_size` rows
+	for (uint8_t i = 0; i != border_size; i++){
+		memcpy(&output_buf[(border_size - 1) * output->w - i * output->w], &output_buf[border_size * output->w + i * output->w], sizeof(uint8_t) * output->w);
+		memcpy(&output_buf[(output->h - border_size) * output->w + i * output->w], &output_buf[(output->h - border_size - 1) * output->w - i * output->w], sizeof(uint8_t) * output->w);
+	}
+}
+
+/**
+ * This function takes previous padded pyramid level and outputs next level of pyramid without padding.
+ * For calculating new pixel value 5x5 filter matrix suggested by Bouguet is used in decimal number form:
+ * [1/16 1/8 3/4 1/8 1/16]' x [1/16 1/8 3/4 1/8 1/16]
+ *
+ * @param[in]  *input  - input image (grayscale only)
+ * @param[out] *output - the output image
+ * @param[in]  border_size  - amount of padding around image. Padding is made by reflecting image elements at the edge
+ * 						      Example: f e d c b a | a b c d e f | f e d c b a
+ */
+void pyramid_next_level(struct image_t *input, struct image_t *output, uint8_t border_size)
+{
+	// Create output image, new image size is half the size of input image without padding (border)
+	image_create(output, (input->w + 1 - 2 * border_size) / 2, (input->h + 1 - 2 * border_size ) / 2, input->type);
+
+	uint8_t *input_buf = (uint8_t *)input->buf;
+	uint8_t *output_buf = (uint8_t *)output->buf;
+
+	uint16_t row, col; // coordinates of the central pixel; pixel being calculated in input matrix; center of filer matrix
+	uint16_t w = input->w;
+	int32_t sum = 0;
+
+	for (uint16_t i = 0; i != output->h; i++){
+
+		for (uint16_t j = 0; j != output->w; j++){
+			row = border_size + 2 * i; // First skip border, then every second pixel
+			col = border_size + 2 * j;
+
+			sum =    39 * ( input_buf[(row -2)*w + (col -2)] + input_buf[(row -2)*w + (col +2)] + input_buf[(row +2)*w + (col -2)] + input_buf[(row +2)*w + (col +2)]);
+			sum +=  156 * ( input_buf[(row -2)*w + (col -1)] + input_buf[(row -2)*w + (col +1)] + input_buf[(row -1)*w + (col +2)] + input_buf[(row +1)*w + (col -2)]
+							 + input_buf[(row +1)*w + (col +2)] + input_buf[(row +2)*w + (col -1)] + input_buf[(row +2)*w + (col +1)] + input_buf[(row -1)*w + (col -2)]);
+			sum +=  234 * ( input_buf[(row -2)*w + (col)] + input_buf[(row)*w    + (col -2)] + input_buf[(row)*w    + (col +2)] + input_buf[(row +2)*w + (col)]);
+			sum +=  625 * ( input_buf[(row -1)*w + (col -1)] + input_buf[(row -1)*w + (col +1)] + input_buf[(row +1)*w + (col -1)] + input_buf[(row +1)*w + (col +1)]);
+			sum +=  938 * ( input_buf[(row -1)*w + (col)] + input_buf[(row)*w    + (col -1)] + input_buf[(row)*w    + (col +1)] + input_buf[(row +1)*w + (col)]);
+			sum += 1406 * input_buf[(row)*w    + (col)];
+
+			output_buf[i*output->w + j] = sum / 10000;
+		}
+	}
+}
+
+
+/**
+ * This function populates given array of image_t structs with wanted number of padded pyramids based on given input.
+ * @param[in]  *input  - input image (grayscale only)
+ * @param[out] *output - array of image_t structs containing image pyiramid levels. Level zero contains original image,
+ *                       followed by `pyr_level` of pyramid.
+ * @param[in]  pyr_level  - number of pyramids to be built. If 0, original image is padded and outputed.
+ * @param[in]  border_size  - amount of padding around image. Padding is made by reflecting image elements at the edge
+ * 						      Example: f e d c b a | a b c d e f | f e d c b a
+ */
+void pyramid_build(struct image_t *input, struct image_t *output_array, uint8_t pyr_level, uint8_t border_size)
+{
+	// Pad input image and save it as '0' pyramid level
+	image_add_border(input, &output_array[0], border_size);
+
+	// Temporary holds 'i' level version of original image to be padded and saved as 'i' pyramid level
+	struct image_t temp;
+
+	for (uint8_t i = 1; i != pyr_level + 1; i++){
+		pyramid_next_level(&output_array[i-1], &temp, border_size);
+		image_add_border(&temp, &output_array[i], border_size);
+		image_free(&temp);
+	}
+}
+
 /**
  * This outputs a subpixel window image in grayscale
  * Currently only works with Grayscale images as input but could be upgraded to
@@ -243,41 +345,45 @@ void image_yuv422_downsample(struct image_t *input, struct image_t *output, uint
  * @param[out] *output Window output (width and height is used to calculate the window size)
  * @param[in] *center Center point in subpixel coordinates
  * @param[in] subpixel_factor The subpixel factor per pixel
+ * @param[in]  border_size  - amount of padding around image. Padding is made by reflecting image elements at the edge
+ * 						      Example: f e d c b a | a b c d e f | f e d c b a
  */
-void image_subpixel_window(struct image_t *input, struct image_t *output, struct point_t *center, uint16_t subpixel_factor)
+void image_subpixel_window(struct image_t *input, struct image_t *output, struct point_t *center, uint32_t subpixel_factor, uint8_t border_size)
 {
   uint8_t *input_buf = (uint8_t *)input->buf;
   uint8_t *output_buf = (uint8_t *)output->buf;
 
   // Calculate the window size
   uint16_t half_window = output->w / 2;
-  uint16_t subpixel_w = input->w * subpixel_factor;
-  uint16_t subpixel_h = input->h * subpixel_factor;
+
+  uint32_t subpixel_w = input->w * subpixel_factor;
+  uint32_t subpixel_h = input->h * subpixel_factor;
 
   // Go through the whole window size in normal coordinates
   for (uint16_t i = 0; i < output->w; i++) {
     for (uint16_t j = 0; j < output->h; j++) {
       // Calculate the subpixel coordinate
-      uint16_t x = center->x + (i - half_window) * subpixel_factor;
-      uint16_t y = center->y + (j - half_window) * subpixel_factor;
-      BoundUpper(x, subpixel_w);
-      BoundUpper(y, subpixel_h);
+      uint32_t x = center->x + border_size * subpixel_factor + (i - half_window) * subpixel_factor ;
+      uint32_t y = center->y + border_size * subpixel_factor + (j - half_window) * subpixel_factor ;
+
+      BoundUpper(x, subpixel_w - 1);
+      BoundUpper(y, subpixel_h - 1);
 
       // Calculate the original pixel coordinate
       uint16_t orig_x = x / subpixel_factor;
       uint16_t orig_y = y / subpixel_factor;
 
       // Calculate top left (in subpixel coordinates)
-      uint16_t tl_x = orig_x * subpixel_factor;
-      uint16_t tl_y = orig_y * subpixel_factor;
+      uint32_t tl_x = orig_x * subpixel_factor;
+      uint32_t tl_y = orig_y * subpixel_factor;
 
       // Check if it is the top left pixel
       if (tl_x == x &&  tl_y == y) {
         output_buf[output->w * j + i] = input_buf[input->w * orig_y + orig_x];
       } else {
         // Calculate the difference from the top left
-        uint16_t alpha_x = (x - tl_x);
-        uint16_t alpha_y = (y - tl_y);
+        uint32_t alpha_x = (x - tl_x);
+        uint32_t alpha_y = (y - tl_y);
 
         // Blend from the 4 surrounding pixels
         uint32_t blend = (subpixel_factor - alpha_x) * (subpixel_factor - alpha_y) * input_buf[input->w * orig_y + orig_x];
@@ -339,7 +445,7 @@ void image_calculate_g(struct image_t *dx, struct image_t *dy, int32_t *g)
     }
   }
 
-  // ouput the G vector
+  // output the G vector
   g[0] = sum_dxx / 255;
   g[1] = sum_dxy / 255;
   g[2] = g[1];
@@ -407,7 +513,7 @@ int32_t image_multiply(struct image_t *img_a, struct image_t *img_b, struct imag
   // Calculate the multiplication
   for (uint16_t x = 0; x < img_a->w; x++) {
     for (uint16_t y = 0; y < img_a->h; y++) {
-      int16_t mult_c = img_a_buf[y * img_a->w + x] * img_b_buf[y * img_b->w + x];
+      int32_t mult_c = img_a_buf[y * img_a->w + x] * img_b_buf[y * img_b->w + x];
       sum += mult_c;
 
       // Set the difference image
diff --git a/sw/airborne/modules/computer_vision/lib/vision/image.h b/sw/airborne/modules/computer_vision/lib/vision/image.h
index ea5ca5009b..cc82875507 100644
--- a/sw/airborne/modules/computer_vision/lib/vision/image.h
+++ b/sw/airborne/modules/computer_vision/lib/vision/image.h
@@ -52,8 +52,8 @@ struct image_t {
 
 /* Image point structure */
 struct point_t {
-  uint16_t x;             ///< The x coordinate of the point
-  uint16_t y;             ///< The y coordinate of the point
+  uint32_t x;             ///< The x coordinate of the point // CHANGED 16 -> 32
+  uint32_t y;             ///< The y coordinate of the point // CHANGED 16 -> 32
 };
 
 /* Vector structure for point differences */
@@ -64,6 +64,7 @@ struct flow_t {
 };
 
 /* Usefull image functions */
+void image_add_border(struct image_t *input, struct image_t *output, uint8_t border_size);
 void image_create(struct image_t *img, uint16_t width, uint16_t height, enum image_type type);
 void image_free(struct image_t *img);
 void image_copy(struct image_t *input, struct image_t *output);
@@ -71,7 +72,7 @@ void image_switch(struct image_t *a, struct image_t *b);
 void image_to_grayscale(struct image_t *input, struct image_t *output);
 uint16_t image_yuv422_colorfilt(struct image_t *input, struct image_t *output, uint8_t y_m, uint8_t y_M, uint8_t u_m, uint8_t u_M, uint8_t v_m, uint8_t v_M);
 void image_yuv422_downsample(struct image_t *input, struct image_t *output, uint16_t downsample);
-void image_subpixel_window(struct image_t *input, struct image_t *output, struct point_t *center, uint16_t subpixel_factor);
+void image_subpixel_window(struct image_t *input, struct image_t *output, struct point_t *center, uint32_t subpixel_factor, uint8_t border_size);
 void image_gradients(struct image_t *input, struct image_t *dx, struct image_t *dy);
 void image_calculate_g(struct image_t *dx, struct image_t *dy, int32_t *g);
 uint32_t image_difference(struct image_t *img_a, struct image_t *img_b, struct image_t *diff);
@@ -79,5 +80,7 @@ int32_t image_multiply(struct image_t *img_a, struct image_t *img_b, struct imag
 void image_show_points(struct image_t *img, struct point_t *points, uint16_t points_cnt);
 void image_show_flow(struct image_t *img, struct flow_t *vectors, uint16_t points_cnt, uint8_t subpixel_factor);
 void image_draw_line(struct image_t *img, struct point_t *from, struct point_t *to);
+void pyramid_next_level(struct image_t *input, struct image_t *output, uint8_t border_size);
+void pyramid_build(struct image_t *input, struct image_t *output_array, uint8_t pyr_level, uint8_t border_size);
 
 #endif
diff --git a/sw/airborne/modules/computer_vision/lib/vision/lucas_kanade.c b/sw/airborne/modules/computer_vision/lib/vision/lucas_kanade.c
index 042cc8c264..8eed35d6e1 100644
--- a/sw/airborne/modules/computer_vision/lib/vision/lucas_kanade.c
+++ b/sw/airborne/modules/computer_vision/lib/vision/lucas_kanade.c
@@ -1,6 +1,14 @@
+/*
+ * lucas_kanade.c
+ *
+ *  Created on: Jan 11, 2016
+ *      Author: hrvoje
+ */
+
 /*
  * Copyright (C) 2014 G. de Croon
  *               2015 Freek van Tienen <freek.v.tienen@gmail.com>
+ *               2016 Hrvoje Brezak <hrvoje.brezak@gmail.com>
  *
  * This file is part of Paparazzi.
  *
@@ -37,7 +45,7 @@
 
 /**
  * Compute the optical flow of several points using the Lucas-Kanade algorithm by Yves Bouguet
- * The initial fixed-point implementation is doen by G. de Croon and is adapted by
+ * The initial fixed-point implementation is done by G. de Croon and is adapted by
  * Freek van Tienen for the implementation in Paparazzi.
  * @param[in] *new_img The newest grayscale image (TODO: fix YUV422 support)
  * @param[in] *old_img The old grayscale image (TODO: fix YUV422 support)
@@ -48,134 +56,177 @@
  * @param[in] max_iterations Maximum amount of iterations to find the new point
  * @param[in] step_threshold The threshold at which the iterations should stop
  * @param[in] max_points The maximum amount of points to track, we skip x points and then take a point.
+ * @param[in] pyramid_level Level of pyramid used in computation (0 == no pyramids used)
  * @return The vectors from the original *points in subpixels
  */
-struct flow_t *opticFlowLK(struct image_t *new_img, struct image_t *old_img, struct point_t *points, uint16_t *points_cnt,
-                           uint16_t half_window_size, uint16_t subpixel_factor, uint8_t max_iterations, uint8_t step_threshold, uint16_t max_points) {
-  // A straightforward one-level implementation of Lucas-Kanade.
-  // For all points:
-  // (1) determine the subpixel neighborhood in the old image
-  // (2) get the x- and y- gradients
-  // (3) determine the 'G'-matrix [sum(Axx) sum(Axy); sum(Axy) sum(Ayy)], where sum is over the window
-  // (4) iterate over taking steps in the image to minimize the error:
-  //     [a] get the subpixel neighborhood in the new image
-  //     [b] determine the image difference between the two neighborhoods
-  //     [c] calculate the 'b'-vector
-  //     [d] calculate the additional flow step and possibly terminate the iteration
+struct flow_t *opticFlowLK(struct image_t *new_img, struct image_t *old_img, struct point_t *points, uint16_t *points_cnt, uint16_t half_window_size,
+		uint16_t subpixel_factor, uint8_t max_iterations, uint8_t step_threshold, uint8_t max_points, uint8_t pyramid_level) {
 
-  // Allocate some memory for returning the vectors
-  struct flow_t *vectors = malloc(sizeof(struct flow_t) * max_points);
-  uint16_t new_p = 0;
-  uint16_t points_orig = *points_cnt;
-  *points_cnt = 0;
 
-  // determine patch sizes and initialize neighborhoods
-  uint16_t patch_size = 2 * half_window_size;
-  uint32_t error_threshold = (25 * 25) *(patch_size *patch_size);
-  uint16_t padded_patch_size = patch_size + 2;
+	// Pyramidal implementation of Lucas-Kanade feature tracker.
+	// For all points:
+	// (1) determine the subpixel neighborhood in the old image
+	// (2) get the x- and y- gradients
+	// (3) determine the 'G'-matrix [sum(Axx) sum(Axy); sum(Axy) sum(Ayy)], where sum is over the window
+	// (4) iterate over taking steps in the image to minimize the error:
+	//     [a] get the subpixel neighborhood in the new image
+	//     [b] determine the image difference between the two neighborhoods
+	//     [c] calculate the 'b'-vector
+	//     [d] calculate the additional flow step and possibly terminate the iteration
 
-  // Create the window images
-  struct image_t window_I, window_J, window_DX, window_DY, window_diff;
-  image_create(&window_I, padded_patch_size, padded_patch_size, IMAGE_GRAYSCALE);
-  image_create(&window_J, patch_size, patch_size, IMAGE_GRAYSCALE);
-  image_create(&window_DX, patch_size, patch_size, IMAGE_GRADIENT);
-  image_create(&window_DY, patch_size, patch_size, IMAGE_GRADIENT);
-  image_create(&window_diff, patch_size, patch_size, IMAGE_GRADIENT);
+	// Allocate some memory for returning the vectors
+	struct flow_t *vectors = malloc(sizeof(struct flow_t) * max_points);
 
-  // Calculate the amount of points to skip
-  float skip_points = (points_orig > max_points) ? points_orig / max_points : 1;
+	// Determine patch sizes and initialize neighborhoods
+	uint16_t patch_size = 2 * half_window_size + 1;
+	uint32_t error_threshold = (25 * 25) * (patch_size * patch_size);
+	uint16_t padded_patch_size = patch_size + 2;
+	uint8_t border_size = padded_patch_size / 2 + 2;
+	step_threshold = step_threshold*(subpixel_factor/100);
 
-  // Go trough all points
-  for (uint16_t i = 0; i < max_points && i < points_orig; i++) {
-    uint16_t p = i * skip_points;
+	// Allocate memory for image pyramids
+	struct image_t *pyramid_old = malloc(sizeof(struct image_t) * (pyramid_level+1));
+	struct image_t *pyramid_new = malloc(sizeof(struct image_t) * (pyramid_level+1));
 
-    // If the pixel is outside ROI, do not track it
-    if (points[p].x < half_window_size || (old_img->w - points[p].x) < half_window_size
-        || points[p].y < half_window_size || (old_img->h - points[p].y) < half_window_size) {
-      continue;
-    }
+	// Build pyramid levels
+	pyramid_build(old_img, pyramid_old, pyramid_level, border_size);
+	pyramid_build(new_img, pyramid_new, pyramid_level, border_size);
 
-    // Convert the point to a subpixel coordinate
-    vectors[new_p].pos.x = points[p].x * subpixel_factor;
-    vectors[new_p].pos.y = points[p].y * subpixel_factor;
-    vectors[new_p].flow_x = 0;
-    vectors[new_p].flow_y = 0;
+	// Create the window images
+	struct image_t window_I, window_J, window_DX, window_DY, window_diff;
+	image_create(&window_I, padded_patch_size, padded_patch_size, IMAGE_GRAYSCALE);
+	image_create(&window_J, patch_size, patch_size, IMAGE_GRAYSCALE);
+	image_create(&window_DX, patch_size, patch_size, IMAGE_GRADIENT);
+	image_create(&window_DY, patch_size, patch_size, IMAGE_GRADIENT);
+	image_create(&window_diff, patch_size, patch_size, IMAGE_GRADIENT);
 
-    // (1) determine the subpixel neighborhood in the old image
-    image_subpixel_window(old_img, &window_I, &vectors[new_p].pos, subpixel_factor);
+	// Iterate through pyramid levels
+	for (int8_t LVL = pyramid_level; LVL != -1; LVL--) {
+		uint16_t points_orig = *points_cnt;
+		*points_cnt = 0;
+		uint16_t new_p = 0;
 
-    // (2) get the x- and y- gradients
-    image_gradients(&window_I, &window_DX, &window_DY);
+		// Calculate the amount of points to skip - disabled until needed
+		float skip_points =	(points_orig > max_points) ? (float)points_orig / max_points : 1;
 
-    // (3) determine the 'G'-matrix [sum(Axx) sum(Axy); sum(Axy) sum(Ayy)], where sum is over the window
-    int32_t G[4];
-    image_calculate_g(&window_DX, &window_DY, G);
+		// Go through all points
+		for (uint16_t i = 0; i < max_points && i < points_orig; i++)
+		{
+			uint16_t p = i * skip_points;
 
-    // calculate G's determinant in subpixel units:
-    int32_t Det = (G[0] * G[3] - G[1] * G[2]) / subpixel_factor;
+			if (LVL == pyramid_level)
+			{
+				// Convert the point to a subpixel coordinate on the top pyramid level
+				vectors[new_p].pos.x = (points[p].x * subpixel_factor) >> pyramid_level;
+				vectors[new_p].pos.y = (points[p].y * subpixel_factor) >> pyramid_level;
+				vectors[new_p].flow_x = 0;
+				vectors[new_p].flow_y = 0;
 
-    // Check if the determinant is bigger than 1
-    if (Det < 1) {
-      continue;
-    }
+			} else {
+				// Convert last pyramid level flow into this pyramid level flow guess
+				vectors[new_p].pos.x = vectors[p].pos.x << 1;
+				vectors[new_p].pos.y = vectors[p].pos.y << 1;
+				vectors[new_p].flow_x = vectors[p].flow_x << 1;
+				vectors[new_p].flow_y = vectors[p].flow_y << 1;
+			}
 
-    // a * (Ax - Bx) + (1-a) * (Ax+1 - Bx+1)
-    // a * Ax - a * Bx + (1-a) * Ax+1 - (1-a) * Bx+1
-    // (a * Ax + (1-a) * Ax+1)  - (a * Bx + (1-a) * Bx+1)
+			// If the pixel is outside ROI, do not track it
+			if ((((int32_t) vectors[new_p].pos.x + vectors[new_p].flow_x) < 0)
+					|| ((vectors[new_p].pos.x + vectors[new_p].flow_x) > ((pyramid_new[LVL].w - 1 - 2 * border_size)* subpixel_factor))
+					|| (((int32_t) vectors[new_p].pos.y + vectors[new_p].flow_y) < 0)
+					|| ((vectors[new_p].pos.y + vectors[new_p].flow_y) > ((pyramid_new[LVL].h - 1 - 2 * border_size)* subpixel_factor)))
+			{
+				continue;
+			}
 
-    // (4) iterate over taking steps in the image to minimize the error:
-    bool_t tracked = TRUE;
-    for (uint8_t it = 0; it < max_iterations; it++) {
-      struct point_t new_point =  {
-        vectors[new_p].pos.x + vectors[new_p].flow_x,
-        vectors[new_p].pos.y + vectors[new_p].flow_y
-      };
-      // If the pixel is outside ROI, do not track it
-      if (new_point.x / subpixel_factor < half_window_size || (old_img->w - new_point.x / subpixel_factor) < half_window_size
-          || new_point.y / subpixel_factor < half_window_size || (old_img->h - new_point.y / subpixel_factor) < half_window_size) {
-        tracked = FALSE;
-        break;
-      }
 
-      //     [a] get the subpixel neighborhood in the new image
-      image_subpixel_window(new_img, &window_J, &new_point, subpixel_factor);
+			// (1) determine the subpixel neighborhood in the old image
+			image_subpixel_window(&pyramid_old[LVL], &window_I, &vectors[new_p].pos, subpixel_factor, border_size);
 
-      //     [b] determine the image difference between the two neighborhoods
-      uint32_t error = image_difference(&window_I, &window_J, &window_diff);
-      if (error > error_threshold && it > max_iterations / 2) {
-        tracked = FALSE;
-        break;
-      }
+			// (2) get the x- and y- gradients
+			image_gradients(&window_I, &window_DX, &window_DY);
 
-      int32_t b_x = image_multiply(&window_diff, &window_DX, NULL) / 255;
-      int32_t b_y = image_multiply(&window_diff, &window_DY, NULL) / 255;
+			// (3) determine the 'G'-matrix [sum(Axx) sum(Axy); sum(Axy) sum(Ayy)], where sum is over the window
+			int32_t G[4];
+			image_calculate_g(&window_DX, &window_DY, G);
 
-      //     [d] calculate the additional flow step and possibly terminate the iteration
-      int16_t step_x = (G[3] * b_x - G[1] * b_y) / Det;
-      int16_t step_y = (G[0] * b_y - G[2] * b_x) / Det;
-      vectors[new_p].flow_x += step_x;
-      vectors[new_p].flow_y += step_y;
+			// calculate G's determinant in subpixel units:
+			int32_t Det = ( G[0] * G[3] - G[1] * G[2]);
 
-      // Check if we exceeded the treshold
-      if ((abs(step_x) + abs(step_y)) < step_threshold) {
-        break;
-      }
-    }
+			// Check if the determinant is bigger than 1
+			if (Det < 1) {
+				continue;
+			}
 
-    // If we tracked the point we update the index and the count
-    if (tracked) {
-      new_p++;
-      (*points_cnt)++;
-    }
-  }
+			// (4) iterate over taking steps in the image to minimize the error:
+			bool_t tracked = TRUE;
 
-  // Free the images
-  image_free(&window_I);
-  image_free(&window_J);
-  image_free(&window_DX);
-  image_free(&window_DY);
-  image_free(&window_diff);
+			for (uint8_t it = max_iterations; it--; ) {
+				struct point_t new_point = { vectors[new_p].pos.x  + vectors[new_p].flow_x,
+											 vectors[new_p].pos.y + vectors[new_p].flow_y };
 
-  // Return the vectors
-  return vectors;
+
+				// If the pixel is outside ROI, do not track it
+				if ( (( (int32_t)vectors[new_p].pos.x  + vectors[new_p].flow_x) < 0)
+						|| ( new_point.x > ((pyramid_new[LVL].w - 1 - 2*border_size)*subpixel_factor))
+						|| (((int32_t)vectors[new_p].pos.y  + vectors[new_p].flow_y) < 0)
+						|| ( new_point.y > ((pyramid_new[LVL].h - 1 - 2*border_size)*subpixel_factor)) )
+				{
+					tracked = FALSE;
+					break;
+				}
+
+				//     [a] get the subpixel neighborhood in the new image
+				image_subpixel_window(&pyramid_new[LVL], &window_J, &new_point, subpixel_factor, border_size);
+
+				//     [b] determine the image difference between the two neighborhoods
+				uint32_t error = image_difference(&window_I, &window_J, &window_diff);
+
+				if (error > error_threshold && it < max_iterations / 2) {
+					tracked = FALSE;
+					break;
+				}
+
+				int32_t b_x = image_multiply(&window_diff, &window_DX, NULL) / 255;
+				int32_t b_y = image_multiply(&window_diff, &window_DY, NULL) / 255;
+
+
+				//     [d] calculate the additional flow step and possibly terminate the iteration
+				int16_t step_x = (( (int64_t) G[3] * b_x - G[1] * b_y) * subpixel_factor) / Det;
+				int16_t step_y = (( (int64_t) G[0] * b_y - G[2] * b_x) * subpixel_factor) / Det;
+
+				vectors[new_p].flow_x = vectors[new_p].flow_x + step_x;
+				vectors[new_p].flow_y = vectors[new_p].flow_y + step_y;
+
+				// Check if we exceeded the treshold CHANGED made this better for 0.03
+				if ((abs(step_x) + abs(step_y)) < step_threshold) {
+					break;
+				}
+			} // lucas kanade step iteration
+
+			// If we tracked the point we update the index and the count
+			if (tracked) {
+				new_p++;
+				(*points_cnt)++;
+			}
+		} // go through all points
+
+	} // LVL of pyramid
+
+	// Free the images
+	image_free(&window_I);
+	image_free(&window_J);
+	image_free(&window_DX);
+	image_free(&window_DY);
+	image_free(&window_diff);
+
+	for (int8_t i = pyramid_level; i!= -1; i--){
+		image_free(&pyramid_old[i]);
+		image_free(&pyramid_new[i]);
+	}
+	pyramid_old = NULL;
+	pyramid_new = NULL;
+
+	// Return the vectors
+	return vectors;
 }
diff --git a/sw/airborne/modules/computer_vision/lib/vision/lucas_kanade.h b/sw/airborne/modules/computer_vision/lib/vision/lucas_kanade.h
index 5a8d0b3da9..7902d644cb 100644
--- a/sw/airborne/modules/computer_vision/lib/vision/lucas_kanade.h
+++ b/sw/airborne/modules/computer_vision/lib/vision/lucas_kanade.h
@@ -34,7 +34,7 @@
 #include "std.h"
 #include "image.h"
 
-struct flow_t *opticFlowLK(struct image_t *new_img, struct image_t *old_img, struct point_t *points, uint16_t *points_cnt,
-                           uint16_t half_window_size, uint16_t subpixel_factor, uint8_t max_iterations, uint8_t step_threshold, uint16_t max_points);
+struct flow_t *opticFlowLK(struct image_t *new_img, struct image_t *old_img, struct point_t *points, uint16_t *points_cnt, uint16_t half_window_size,
+                            uint16_t subpixel_factor, uint8_t max_iterations, uint8_t step_threshold, uint8_t max_points, uint8_t pyramid_level);
 
 #endif /* OPTIC_FLOW_INT_H */
diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c
index ab3828ab41..3055f63801 100644
--- a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c
+++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c
@@ -110,7 +110,7 @@ PRINT_CONFIG_VAR(OPTICFLOW_THRESHOLD_VEC)
 PRINT_CONFIG_VAR(OPTICFLOW_FAST9_ADAPTIVE)
 
 #ifndef OPTICFLOW_FAST9_THRESHOLD
-#define OPTICFLOW_FAST9_THRESHOLD 20
+#define OPTICFLOW_FAST9_THRESHOLD 10
 #endif
 PRINT_CONFIG_VAR(OPTICFLOW_FAST9_THRESHOLD)
 
@@ -206,7 +206,7 @@ void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_sta
 
   // FAST corner detection (TODO: non fixed threshold)
   struct point_t *corners = fast9_detect(img, opticflow->fast9_threshold, opticflow->fast9_min_distance,
-                                         20, 20, &result->corner_cnt);
+                                         0, 0, &result->corner_cnt);
 
   // Adaptive threshold
   if (opticflow->fast9_adaptive) {
@@ -238,7 +238,8 @@ void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_sta
   result->tracked_cnt = result->corner_cnt;
   struct flow_t *vectors = opticFlowLK(&opticflow->img_gray, &opticflow->prev_img_gray, corners, &result->tracked_cnt,
                                        opticflow->window_size / 2, opticflow->subpixel_factor, opticflow->max_iterations,
-                                       opticflow->threshold_vec, opticflow->max_track_corners);
+                                       opticflow->threshold_vec, opticflow->max_track_corners, 3);
+
 
 #if OPTICFLOW_DEBUG && OPTICFLOW_SHOW_FLOW
   image_show_flow(img, vectors, result->tracked_cnt, opticflow->subpixel_factor);
@@ -299,6 +300,10 @@ void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_sta
   float diff_flow_x = 0;
   float diff_flow_y = 0;
 
+  // Flow Derotation TODO:
+  float diff_flow_x = (state->phi - opticflow->prev_phi) * img->w / OPTICFLOW_FOV_W;
+  float diff_flow_y = (state->theta - opticflow->prev_theta) * img->h / OPTICFLOW_FOV_H;
+
   if (opticflow->derotation) {
     diff_flow_x = (state->phi - opticflow->prev_phi) * img->w / OPTICFLOW_FOV_W;
     diff_flow_y = (state->theta - opticflow->prev_theta) * img->h / OPTICFLOW_FOV_H;
diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h
index 1a642601f6..0e6a1a45f1 100644
--- a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h
+++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h
@@ -48,7 +48,7 @@ struct opticflow_t {
   uint16_t search_distance;         ///< Search distance for blockmatching alg.
   uint8_t derotation;             ///< Derotation switched on or off (depended on the quality of the gyroscope measurement)
 
-  uint8_t subpixel_factor;          ///< The amount of subpixels per pixel
+  uint16_t subpixel_factor;          ///< The amount of subpixels per pixel
   uint8_t max_iterations;           ///< The maximum amount of iterations the Lucas Kanade algorithm should do
   uint8_t threshold_vec;            ///< The threshold in x, y subpixels which the algorithm should stop
 
@@ -61,8 +61,8 @@ struct opticflow_t {
 
 
 void opticflow_calc_init(struct opticflow_t *opticflow, uint16_t w, uint16_t h);
-void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
-                          struct opticflow_result_t *result);
+void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img, struct opticflow_result_t *result);
+
 void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
                              struct opticflow_result_t *result);
 void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,