diff --git a/conf/modules/cv_georeference.xml b/conf/modules/cv_georeference.xml
new file mode 100644
index 0000000000..8b87d87474
--- /dev/null
+++ b/conf/modules/cv_georeference.xml
@@ -0,0 +1,23 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="cv_georeference" dir="computer_vision">
+  <doc>
+    <description>Geo-reference computer vision detections</description>
+  </doc>
+  <settings>
+    <dl_settings>
+      <dl_settings name="georeference">
+        <dl_setting var="focus_length" min="1" step="10" max="4000" shortname="Focus"/>
+      </dl_settings>
+    </dl_settings>
+  </settings>
+  <header>
+    <file name="cv_georeference.h"/>
+  </header>
+  <init fun="georeference_init()"/>
+  <periodic fun="georeference_run()" freq="4" start="georeference_init()" autorun="TRUE" />
+  <makefile>
+    <file name="cv_georeference.c"/>
+  </makefile>
+</module>
+
diff --git a/sw/airborne/modules/computer_vision/cv_georeference.c b/sw/airborne/modules/computer_vision/cv_georeference.c
new file mode 100644
index 0000000000..97063319bb
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/cv_georeference.c
@@ -0,0 +1,184 @@
+/*
+ * Copyright (C) C. De Wagter
+ *
+ * This file is part of paparazzi
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+/**
+ * @file "modules/computer_vision/cv_georeference.c"
+ * @author C. De Wagter
+ * Geo-reference computer vision detections
+ */
+
+#include "modules/computer_vision/cv_georeference.h"
+
+#include "math/pprz_trig_int.h"
+#include "math/pprz_algebra.h"
+#include "math/pprz_algebra_int.h"
+
+#include "state.h"
+#include "generated/flight_plan.h"
+
+struct camera_frame_t {
+  int32_t w;     ///< Frame width [px]
+  int32_t h;     ///< Frame height [px]
+  int32_t f;     ///< Camera Focal length in [px]
+  int32_t px;    ///< Target pixel coordinate (left = 0)
+  int32_t py;    ///< Target pixel coordinate (top = 0)
+};
+
+struct georeference_filter_t {
+  struct Int32Vect3 x;          ///< Target
+  struct Int32Vect3 v;          ///< Target Velocity
+  int32_t P;                    ///< Covariance/Average-count
+};
+
+struct georeference_t {
+  struct Int32Vect3 target_i;   ///< Target in pixels, with z being the focal length in pixels, x=up,y=right,out
+  struct Int32Vect3 target_l;   ///< Target in meters, relative to the drone in LTP frame
+
+  struct Int32Vect3 x_t;        ///< Target coordinates NED
+
+  struct georeference_filter_t filter;  ///< Filter waypoint location
+};
+
+struct georeference_t geo;
+
+void georeference_project(struct camera_frame_t *tar, int wp)
+{
+  // Target direction in camera frame: Zero is looking down in body frames
+  // Pixel with x (width) value 0 projects to the left (body-Y-axis)
+  // and y = 0 (height) to the top (body-X-axis)
+  VECT3_ASSIGN(geo.target_i,
+               ((tar->h / 2) - tar->py),
+               (tar->px - (tar->w / 2)),
+               (tar->f)
+              );
+  INT32_VECT3_LSHIFT(geo.target_i, geo.target_i, 4)
+
+  // Camera <-> Body
+  // Looking down in body frame
+  // Bebop has 180deg Z rotation in camera (butt up yields normal webcam)
+  struct Int32RMat body_to_cam_rmat;
+  INT32_MAT33_ZERO(body_to_cam_rmat);
+  MAT33_ELMT(body_to_cam_rmat, 0, 0) = -1 << INT32_TRIG_FRAC;
+  MAT33_ELMT(body_to_cam_rmat, 1, 1) = -1 << INT32_TRIG_FRAC;
+  MAT33_ELMT(body_to_cam_rmat, 2, 2) =  1 << INT32_TRIG_FRAC;
+
+  struct Int32Vect3 target_b;
+  int32_rmat_transp_vmult(&target_b, &body_to_cam_rmat, &geo.target_i);
+
+  // Body <-> LTP
+  struct Int32RMat *ltp_to_body_rmat = stateGetNedToBodyRMat_i();
+  int32_rmat_transp_vmult(&geo.target_l, ltp_to_body_rmat, &target_b);
+
+  // target_l is now a scale-less [pix<<POS_FRAC] vector in LTP from the drone to the target
+  // Divide by z-component to normalize the projection vector
+  int32_t zi = geo.target_l.z;
+  if (zi <= 0)
+  {
+    // Pointing up or horizontal -> no ground projection
+    return;
+  }
+
+  // Multiply with height above ground
+  struct NedCoor_i *pos = stateGetPositionNed_i();
+  int32_t zb = pos->z;
+  geo.target_l.x *= zb;
+  geo.target_l.y *= zb;
+
+  // Divide by z-component
+  geo.target_l.x /= zi;
+  geo.target_l.y /= zi;
+  geo.target_l.z = zb;
+
+  // NED
+  geo.x_t.x = pos->x - geo.target_l.x;
+  geo.x_t.y = pos->y - geo.target_l.y;
+  geo.x_t.z = 0;
+
+  // ENU
+  waypoint_set_xy_i(wp, geo.x_t.y, geo.x_t.x);
+  waypoint_set_alt_i(wp, geo.x_t.z);
+}
+
+void georeference_filter(bool_t kalman, int wp)
+{
+  struct Int32Vect3 err;
+
+  if (kalman)
+  {
+    // Predict
+    VECT3_ADD(geo.filter.x, geo.filter.v);
+
+    // Error = prediction - observation
+    VECT3_COPY(err,geo.filter.x);
+    VECT3_SUB(err, geo.x_t);
+  }
+  else // Average
+  {
+    VECT3_SMUL(geo.filter.x,geo.filter.x,geo.filter.P);
+    VECT3_ADD(geo.filter.x, geo.x_t);
+    geo.filter.P++;
+    VECT3_SDIV(geo.filter.x,geo.filter.x,geo.filter.P);
+  }
+
+  // ENU
+  waypoint_set_xy_i(wp, geo.filter.x.y, geo.filter.x.x);
+  waypoint_set_alt_i(wp, geo.filter.x.z);
+}
+
+int32_t focus_length;
+
+void georeference_run(void)
+{
+  struct camera_frame_t target;
+  target.w = 320;
+  target.h = 240;
+  target.f = focus_length;
+  target.px = 0;
+  target.py = 0;
+  georeference_project(&target,WP_p1);
+  target.px = 320;
+  target.py = 0;
+  georeference_project(&target,WP_p2);
+  target.px = 320;
+  target.py = 240;
+  georeference_project(&target,WP_p3);
+  target.px = 0;
+  target.py = 240;
+  georeference_project(&target,WP_p4);
+
+  target.px = 0;
+  target.py = 120;
+  georeference_project(&target,0);
+  georeference_filter(FALSE, WP_CAM);
+}
+
+void georeference_init(void)
+{
+  INT32_VECT3_ZERO(geo.target_i);
+  INT32_VECT3_ZERO(geo.target_l);
+
+  VECT3_ASSIGN(geo.x_t, 0, 0, 0);
+
+  INT32_VECT3_ZERO(geo.filter.v);
+  INT32_VECT3_ZERO(geo.filter.x);
+  geo.filter.P = 0;
+  focus_length = 400;
+}
+
+
diff --git a/sw/airborne/modules/computer_vision/cv_georeference.h b/sw/airborne/modules/computer_vision/cv_georeference.h
new file mode 100644
index 0000000000..56e134a140
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/cv_georeference.h
@@ -0,0 +1,37 @@
+/*
+ * Copyright (C) C. De Wagter
+ *
+ * This file is part of paparazzi
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+/**
+ * @file "modules/computer_vision/cv_georeference.h"
+ * @author C. De Wagter
+ * Geo-reference computer vision detections
+ */
+
+#ifndef CV_GEOREFERENCE_H
+#define CV_GEOREFERENCE_H
+
+#include <stdint.h>
+
+extern int32_t focus_length;
+
+extern void georeference_init(void);
+extern void georeference_run(void);
+
+#endif
+