diff --git a/conf/airframes/TUDELFT/tudelft_bebop2_indi_opticflow.xml b/conf/airframes/TUDELFT/tudelft_bebop2_indi_opticflow.xml
index 645831409b..d3e63b3cef 100644
--- a/conf/airframes/TUDELFT/tudelft_bebop2_indi_opticflow.xml
+++ b/conf/airframes/TUDELFT/tudelft_bebop2_indi_opticflow.xml
@@ -1,6 +1,6 @@
 <!DOCTYPE airframe SYSTEM "../airframe.dtd">
 
-<airframe name="bebop2_indi">
+<airframe name="bebop2_indi_opticflow">
 
   <firmware name="rotorcraft">
     <target name="ap" board="bebop2">
@@ -43,8 +43,6 @@
 	<define name="OPTICFLOW_METHOD" value="1"/>
 	<define name="OPTICFLOW_SHOW_FLOW" value="1"/>
 	<define name="MAX_HORIZON" value="2"/>
-	<define name="IMAGE_HEIGHT" value="480"/>
-	<define name="IMAGE_WIDTH" value="640"/>
     </load>
     <!--load name="video_rtp_stream.xml">
 	<define name="VIEWVIDEO_DOWNSIZE_FACTOR" value="1"/>
diff --git a/sw/airborne/modules/computer_vision/lib/vision/edge_flow.h b/sw/airborne/modules/computer_vision/lib/vision/edge_flow.h
index 032608996e..ef2f710020 100644
--- a/sw/airborne/modules/computer_vision/lib/vision/edge_flow.h
+++ b/sw/airborne/modules/computer_vision/lib/vision/edge_flow.h
@@ -43,6 +43,10 @@
 
 #ifndef MAX_HORIZON
 #define MAX_HORIZON 10
+#else
+#if MAX_HORIZON < 2
+#define MAX_HORIZON 2
+#endif
 #endif
 #ifndef DISP_RANGE_MAX
 #define DISP_RANGE_MAX 50
@@ -50,12 +54,6 @@
 #ifndef MAX_WINDOW_SIZE
 #define MAX_WINDOW_SIZE 20
 #endif
-#ifndef IMAGE_HEIGHT
-#define IMAGE_HEIGHT 240
-#endif
-#ifndef IMAGE_WIDTH
-#define IMAGE_WIDTH  360
-#endif
 #ifndef OPTICFLOW_FOV_W
 #define OPTICFLOW_FOV_W 0.89360857702
 #endif
@@ -64,16 +62,16 @@
 #endif
 
 struct edge_hist_t {
-  int32_t x[IMAGE_WIDTH];
-  int32_t y[IMAGE_HEIGHT];
+  int32_t *x;
+  int32_t *y;
   struct timeval frame_time;
   float roll;
   float pitch;
 };
 
 struct edgeflow_displacement_t {
-  int32_t x[IMAGE_WIDTH];
-  int32_t y[IMAGE_HEIGHT];
+  int32_t *x;
+  int32_t *y;
 };
 
 struct edge_flow_t {
@@ -86,7 +84,7 @@ struct edge_flow_t {
 
 // Local functions of the EDGEFLOW algorithm
 void draw_edgeflow_img(struct image_t *img, struct edge_flow_t edgeflow, int32_t *edge_hist_x_prev
-                       ,int32_t *edge_hist_x);
+                       , int32_t *edge_hist_x);
 void calc_previous_frame_nr(struct opticflow_result_t *result, struct opticflow_t *opticflow, uint8_t current_frame_nr,
                             uint8_t *previous_frame_offset, uint8_t *previous_frame_nr);
 void calculate_edge_histogram(struct image_t *img, int32_t edge_histogram[],
diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c
index 159b5a59b4..481a1c78e0 100644
--- a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c
+++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c
@@ -187,6 +187,10 @@ void opticflow_calc_init(struct opticflow_t *opticflow, uint16_t w, uint16_t h)
 void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
                              struct opticflow_result_t *result)
 {
+  if (opticflow->just_switched_method == 1) {
+    opticflow_calc_init(opticflow, img->w, img->h);
+  }
+
   // variables for size_divergence:
   float size_divergence; int n_samples;
 
@@ -365,11 +369,26 @@ void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t *
   // Define Static Variables
   static struct edge_hist_t edge_hist[MAX_HORIZON];
   static uint8_t current_frame_nr = 0;
-  static struct edge_flow_t edgeflow;
+  struct edge_flow_t edgeflow;
   static uint8_t previous_frame_offset[2] = {1, 1};
 
   // Define Normal variables
   struct edgeflow_displacement_t displacement;
+  displacement.x = malloc(sizeof(int32_t) * img->w);
+  displacement.y = malloc(sizeof(int32_t) * img->h);
+
+  // If the methods just switched to this one, reintialize the
+  // array of edge_hist structure.
+  if (opticflow->just_switched_method == 1) {
+    int i;
+    for (i = 0; i < MAX_HORIZON; i++) {
+      edge_hist[i].x = malloc(sizeof(int32_t) * img->w);
+      edge_hist[i].y = malloc(sizeof(int32_t) * img->h);
+      edge_hist[i].roll = 0.0f;
+      edge_hist[i].pitch = 0.0f;
+    }
+  }
+
   uint16_t disp_range;
   if (opticflow->search_distance < DISP_RANGE_MAX) {
     disp_range = opticflow->search_distance;
@@ -395,6 +414,7 @@ void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t *
   calculate_edge_histogram(img, edge_hist_x, 'x', 0);
   calculate_edge_histogram(img, edge_hist_y, 'y', 0);
 
+
   // Copy frame time and angles of image to calculated edge histogram
   memcpy(&edge_hist[current_frame_nr].frame_time, &img->ts, sizeof(struct timeval));
   edge_hist[current_frame_nr].pitch = state->theta;
@@ -506,6 +526,14 @@ void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t *
 void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
                           struct opticflow_result_t *result)
 {
+  static int8_t switch_counter = OPTICFLOW_METHOD;
+  if (switch_counter != opticflow->method || opticflow->got_first_img) {
+    opticflow->just_switched_method = 1;
+    switch_counter = opticflow->method;
+  } else {
+    opticflow->just_switched_method = 0;
+  }
+
   if (opticflow->method == 0) {
     calc_fast9_lukas_kanade(opticflow, state, img, result);
   } else {
@@ -513,6 +541,11 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_
       calc_edgeflow_tot(opticflow, state, img, result);
     } else {}
   }
+
+  if (opticflow->got_first_img == 1) {
+    opticflow->got_first_img = 0;
+  }
+
 }
 
 /**
diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h
index a55302ef24..6fc1a23313 100644
--- a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h
+++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h
@@ -37,6 +37,7 @@
 
 struct opticflow_t {
   bool got_first_img;             ///< If we got a image to work with
+  bool just_switched_method;
   float prev_phi;                   ///< Phi from the previous image frame
   float prev_theta;                 ///< Theta from the previous image frame
   struct image_t img_gray;          ///< Current gray image frame
diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c
index 8a05c56b73..c494575fc1 100644
--- a/sw/airborne/modules/computer_vision/opticflow_module.c
+++ b/sw/airborne/modules/computer_vision/opticflow_module.c
@@ -94,9 +94,8 @@ void opticflow_module_init(void)
   opticflow_state.agl = 0;
 
   // Initialize the opticflow calculation
-  // TODO: make it independable on img size!
-  opticflow_calc_init(&opticflow, 640, 480);
   opticflow_got_result = false;
+  opticflow.got_first_img = 1;
   cv_add(opticflow_module_calc);
 
 #if PERIODIC_TELEMETRY
@@ -152,7 +151,7 @@ struct image_t *opticflow_module_calc(struct image_t *img)
   memcpy(&temp_state, &opticflow_state, sizeof(struct opticflow_state_t));
 
   // Do the optical flow calculation
-  opticflow_calc_frame(&opticflow, &temp_state, img, &opticflow_result);
+  opticflow_calc_frame(&opticflow, &opticflow_state, img, &opticflow_result);
 
   // Copy the result if finished
 // memcpy(&opticflow_result, &temp_result, sizeof(struct opticflow_result_t));
@@ -171,5 +170,7 @@ static void opticflow_agl_cb(uint8_t sender_id __attribute__((unused)), float di
   // Update the distance if we got a valid measurement
   if (distance > 0) {
     opticflow_state.agl = distance;
+
+
   }
 }