diff --git a/include/sensors/data_source_camera_conicmirror.h b/include/sensors/data_source_camera_conicmirror.h
index e3fdf9d..7ade254 100644
--- a/include/sensors/data_source_camera_conicmirror.h
+++ b/include/sensors/data_source_camera_conicmirror.h
@@ -1,6 +1,7 @@
 #pragma once
 
 #include "behaviour_control/data_source.h"
+#include "behaviour_control/complementary_filter.h"
 
 #define startp 105
 #define endp 115
@@ -11,6 +12,12 @@
 //Imu To Camera Angle Mult
 #define IMUTOC_AMULT 1
 
+#define FILTER_DEFAULT_COEFF 0.6
+#define FILTER_BY_COEFF FILTER_DEFAULT_COEFF
+#define FILTER_BX_COEFF FILTER_DEFAULT_COEFF
+#define FILTER_YY_COEFF FILTER_DEFAULT_COEFF
+#define FILTER_YX_COEFF FILTER_DEFAULT_COEFF
+
 class DataSourceCameraConic : public DataSource{
 
     public:
@@ -30,4 +37,6 @@ class DataSourceCameraConic : public DataSource{
 
         int goalOrientation, pAtk, pDef;
         int value;
+
+        ComplementaryFilter *filter_yy, *filter_xy, *filter_yb, *filter_xb, *filter_yy_fix, *filter_xy_fix, *filter_yb_fix, *filter_xb_fix;
 };
\ No newline at end of file
diff --git a/include/systems/position/positionsys_camera.h b/include/systems/position/positionsys_camera.h
index cc7cbf9..4bbddf8 100644
--- a/include/systems/position/positionsys_camera.h
+++ b/include/systems/position/positionsys_camera.h
@@ -7,8 +7,8 @@
 /*Camera translation: because of mechanical imprecision, the center of the camera and the center of the cone mirror may not coincide
 To overcome this, each coordinate needs to be shifted by some amount, defined on a per-robot basis that needs to be recalibrated each time.
 These values need to be subtracted from the coords used in setMoveSetpoints*/
-#define CAMERA_TRANSLATION_X 8
-#define CAMERA_TRANSLATION_Y 10
+#define CAMERA_TRANSLATION_X 10
+#define CAMERA_TRANSLATION_Y 0
 //Camera center: those setpoints correspond to what we consider the center of the field
 #define CAMERA_CENTER_X 0
 #define CAMERA_CENTER_Y 0
@@ -24,7 +24,7 @@ These values need to be subtracted from the coords used in setMoveSetpoints*/
 //Actually it's ± MAX_VAL
 #define MAX_X 50
 #define MAX_Y (CAMERA_CENTER_Y_ABS_SUM/2)
-#define DIST_MULT 1.4
+#define DIST_MULT 1.7
 
 #define Kpx 1
 #define Kix 0
diff --git a/src/sensors/data_source_camera_conicmirror.cpp b/src/sensors/data_source_camera_conicmirror.cpp
index e463695..d904274 100644
--- a/src/sensors/data_source_camera_conicmirror.cpp
+++ b/src/sensors/data_source_camera_conicmirror.cpp
@@ -1,6 +1,9 @@
 #include "behaviour_control/status_vector.h"
 #include "sensors/data_source_camera_conicmirror.h"
 
+//Comment out to disable complementary filters on angles
+#define CAMERA_CONIC_FILTER_POINTS
+
 DataSourceCameraConic::DataSourceCameraConic(HardwareSerial *ser_, int baud) : DataSource(ser_, baud) {
   true_xb = 0;
   true_yb = 0;
@@ -27,6 +30,15 @@ DataSourceCameraConic::DataSourceCameraConic(HardwareSerial *ser_, int baud) : D
   true_yb_fixed = 0;
   true_xy_fixed = 0;
   true_yy_fixed = 0;
+
+  filter_yy = new ComplementaryFilter(FILTER_YY_COEFF);
+  filter_xy = new ComplementaryFilter(FILTER_YX_COEFF);
+  filter_yb = new ComplementaryFilter(FILTER_BY_COEFF);
+  filter_xb = new ComplementaryFilter(FILTER_BX_COEFF);
+  filter_yy_fix = new ComplementaryFilter(FILTER_YY_COEFF);
+  filter_xy_fix = new ComplementaryFilter(FILTER_YX_COEFF);
+  filter_yb_fix = new ComplementaryFilter(FILTER_BY_COEFF);
+  filter_xb_fix = new ComplementaryFilter(FILTER_BX_COEFF);
 }
 
 void DataSourceCameraConic ::readSensor() {
@@ -76,6 +88,13 @@ void DataSourceCameraConic ::computeCoordsAngles() {
   true_xy = 50 - true_xy;
   true_yy = true_yy - 50;
 
+  #ifdef CAMERA_CONIC_FILTER_POINTS
+  true_xb = filter_xb->calculate(true_xb);
+  true_yb = filter_yb->calculate(true_yb);
+  true_xy = filter_xy->calculate(true_xy);
+  true_yy = filter_yy->calculate(true_yy);
+  #endif
+
   //-90 + to account for phase shifting with goniometric circle
   yAngle = -90 + (atan2(true_yy, true_xy) * 180 / 3.14);
   bAngle = -90 + (atan2(true_yb, true_xb) * 180 / 3.14);
@@ -104,6 +123,13 @@ void DataSourceCameraConic ::computeCoordsAngles() {
   yAngleFix = 90 - (atan2(true_yy_fixed, true_xy_fixed) * 180 / 3.14);
   bAngleFix = 90 - (atan2(true_yb_fixed, true_xb_fixed) * 180 / 3.14);
 
+  #ifdef CAMERA_CONIC_FILTER_POINTS
+  true_xb_fixed = filter_xb_fix->calculate(true_xb_fixed);
+  true_yb_fixed = filter_yb_fix->calculate(true_yb_fixed);
+  true_xy_fixed = filter_xy_fix->calculate(true_xy_fixed);
+  true_yy_fixed = filter_yy_fix->calculate(true_yy_fixed);
+  #endif
+
   //Important: update status vector
   CURRENT_INPUT_WRITE.cameraByte = value;
   CURRENT_DATA_WRITE.cam_xb = true_xb;
diff --git a/src/system/positions/positionsys_camera.cpp b/src/system/positions/positionsys_camera.cpp
index 321aa71..f49ad28 100644
--- a/src/system/positions/positionsys_camera.cpp
+++ b/src/system/positions/positionsys_camera.cpp
@@ -2,6 +2,7 @@
 #include "systems/position/positionsys_camera.h"
 #include "sensors/sensors.h"
 #include "vars.h"
+#include "math.h"
 
 PositionSysCamera::PositionSysCamera() {
     MAX_DIST = sqrt(MAX_X*MAX_X + MAX_Y*MAX_Y);
@@ -112,17 +113,18 @@ void PositionSysCamera::CameraPID(){
         Setpointy += axisy;
 
         X->Compute();
-        Y->Compute();   
+        Y->Compute();
         
         //Compute an X and Y to give to the PID later
         //There's surely a better way to do this
         int dir = -90-(atan2(Outputy,Outputx)*180/3.14);
         dir = (dir+360) % 360;
 
-        int dist = sqrt(Outputx*Outputx + Outputy*Outputy);
+        int dist = sqrt( ( (CURRENT_DATA_WRITE.posx-Setpointx)*(CURRENT_DATA_WRITE.posx-Setpointx) ) + (CURRENT_DATA_WRITE.posy-Setpointy)*(CURRENT_DATA_WRITE.posy-Setpointy) );
+        // int dist = sqrt(Outputx*Outputx + Outputy*Outputy);
         int speed = map(dist*DIST_MULT, 0, MAX_DIST, 0,  MAX_VEL);
         speed = filterSpeed->calculate(speed);
-        speed = speed > 35 ? speed : 0;
+        speed = speed > 40 ? speed : 0;
         dir = filterDir->calculate(dir);
         // drive->prepareDrive(dir, speed, 0);
 
diff --git a/src/test_menu.cpp b/src/test_menu.cpp
index 501b8d7..ca1eaa0 100644
--- a/src/test_menu.cpp
+++ b/src/test_menu.cpp
@@ -13,6 +13,8 @@
 #include "behaviour_control/data_source.h"
 #include "behaviour_control/status_vector.h"
 
+unsigned long timer = 0;
+
 void TestMenu::testMenu()
 {
     if (!(DEBUG.available() || flagtest))
@@ -91,6 +93,16 @@ void TestMenu::testMenu()
             DEBUG.println(robot_indentifier);
             delay(50);
             break;
+        case 'p':
+            if(millis() - timer > 150){
+                timer = millis();
+                DEBUG.println("Position on the field (camera)");
+                DEBUG.print("X: ");
+                DEBUG.print(CURRENT_DATA_READ.posx);
+                DEBUG.print(" , Y: ");
+                DEBUG.println(CURRENT_DATA_READ.posy);
+            }
+            break;
         case 'u':
             DEBUG.println("32u4 receive Test");
             while (Serial2.available())
diff --git a/utility/OpenMV/conic_eff_h7.py b/utility/OpenMV/conic_eff_h7.py
index a246126..9e01fbc 100644
--- a/utility/OpenMV/conic_eff_h7.py
+++ b/utility/OpenMV/conic_eff_h7.py
@@ -45,11 +45,11 @@ blue_led.on()
 ##############################################################################
 
 
-thresholds = [  (48, 73, -15, 15, 29, 79),    # thresholds yellow goal
-                (12, 43, -15, 15, -41, -9)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
+thresholds = [  (54, 73, -4, 17, 18, 73),    # thresholds yellow goal
+                (36, 45, -7, 7, -35, -10)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
 
 
-roi = (50,5,250, 230)
+roi = (80, 0, 240, 230)
 
 # Camera Setup ###############################################################
 '''sensor.reset()xxxx
@@ -69,7 +69,7 @@ sensor.set_pixformat(sensor.RGB565)
 sensor.set_framesize(sensor.QVGA)
 sensor.set_windowing(roi)
 sensor.set_contrast(0)
-sensor.set_saturation(3)
+sensor.set_saturation(0)
 sensor.set_brightness(1)
 sensor.set_auto_whitebal(True)
 sensor.set_auto_exposure(False, 6576)