diff --git a/conf/airframes/tudelft/nederdrone8.xml b/conf/airframes/tudelft/nederdrone8.xml
index b28d3d3b94..9b5f74ef15 100644
--- a/conf/airframes/tudelft/nederdrone8.xml
+++ b/conf/airframes/tudelft/nederdrone8.xml
@@ -90,6 +90,7 @@
 
     <module name="approach_moving_target">
       <define name="AMT_ERR_SLOWDOWN_GAIN" value="0.25"/>
+      <define name="CUTOFF_FREQ_FILTERS_HZ" value="0.25"/>
     </module>
 
     <module name="ins"           type="ekf2">
diff --git a/conf/modules/approach_moving_target.xml b/conf/modules/approach_moving_target.xml
index f801b933f8..7f3e968e48 100644
--- a/conf/modules/approach_moving_target.xml
+++ b/conf/modules/approach_moving_target.xml
@@ -5,6 +5,8 @@
     <description>
       Approach a moving target (e.g. ship) along a diagonal.
     </description>
+    <define name="AMT_ERR_SLOWDOWN_GAIN" value="0.25" description="slowing down when arriving"/>
+    <define name="CUTOFF_FREQ_FILTERS_HZ" value="0.5" description="low pass filter on WP position"/>
   </doc>
   <settings>
     <dl_settings>
@@ -13,10 +15,11 @@
         <dl_setting var="amt.slope_ref" min="0." max="80." step="1.0" shortname="slope_app" unit="degrees"/>
         <dl_setting var="amt.speed" min="-5.0" max="5." step="0.1" shortname="speed" unit="m/s"/>
         <dl_setting var="amt.distance" min="0.0" max="200." step="1.0" shortname="dist" unit="m"/>
-        <dl_setting var="amt_err_slowdown_gain" min="0.0" max="4." step="0.01" shortname="slwdn_gain"/>
+        <dl_setting var="amt_err_slowdown_gain" min="0.0" max="4." step="0.01" shortname="slwdn_gain" param="AMT_ERR_SLOWDOWN_GAIN"/>
         <dl_setting var="amt.pos_gain" min="0.05" max="3.0" step="0.01" shortname="pos_gain"/>
         <dl_setting var="amt.speed_gain" min="0.0" max="1.0" step="0.01" shortname="speed_gain"/>
         <dl_setting var="amt.relvel_gain" min="0.0" max="1.0" step="0.01" shortname="relvel_gain"/>
+        <dl_setting var="cutoff_freq_filters_hz" min="0.3" max="2.0" step="0.01" shortname="filter_freq" module="ctrl/approach_moving_target" handler="set_low_pass_freq" param="CUTOFF_FREQ_FILTERS_HZ"/>
       </dl_settings>
     </dl_settings>
   </settings>
diff --git a/sw/airborne/modules/ctrl/approach_moving_target.c b/sw/airborne/modules/ctrl/approach_moving_target.c
index 871783f577..62895ce227 100644
--- a/sw/airborne/modules/ctrl/approach_moving_target.c
+++ b/sw/airborne/modules/ctrl/approach_moving_target.c
@@ -27,11 +27,29 @@
 
 #include "generated/modules.h"
 #include "modules/core/abi.h"
+#include "filters/low_pass_filter.h"
+
+
+#ifndef AMT_ERR_SLOWDOWN_GAIN
+#define AMT_ERR_SLOWDOWN_GAIN 0.25
+#endif
+
+// Filter value in Hz
+#ifndef CUTOFF_FREQ_FILTERS_HZ
+#define CUTOFF_FREQ_FILTERS_HZ 0.5
+#endif 
+
 
 float amt_err_slowdown_gain = AMT_ERR_SLOWDOWN_GAIN;
 
 float approach_moving_target_angle_deg;
 
+float cutoff_freq_filters_hz = CUTOFF_FREQ_FILTERS_HZ;
+
+Butterworth2LowPass target_pos_filt[3];
+Butterworth2LowPass target_vel_filt[3];
+Butterworth2LowPass target_heading_filt;
+
 #define DEBUG_AMT TRUE
 #include <stdio.h>
 
@@ -75,11 +93,25 @@ static void send_approach_moving_target(struct transport_tx *trans, struct link_
 
 void approach_moving_target_init(void)
 {
+  approach_moving_target_set_low_pass_freq(cutoff_freq_filters_hz);
 #if PERIODIC_TELEMETRY
   register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_APPROACH_MOVING_TARGET, send_approach_moving_target);
 #endif
 }
 
+void approach_moving_target_set_low_pass_freq(float filter_freq) {
+  cutoff_freq_filters_hz = filter_freq;
+  // tau = 1/(2*pi*Fc)
+  float tau = 1.0 / (2.0 * M_PI * cutoff_freq_filters_hz);
+  float sample_time = 1.0 / FOLLOW_DIAGONAL_APPROACH_FREQ;
+  for (uint8_t i = 0; i < 3; i++) {
+    init_butterworth_2_low_pass(&target_pos_filt[i], tau, sample_time, 0.0);
+    init_butterworth_2_low_pass(&target_vel_filt[i], tau, sample_time, 0.0);
+  }
+  init_butterworth_2_low_pass(&target_heading_filt, tau, sample_time, 0.0);
+
+}
+
 // Update a waypoint such that you can see on the GCS where the drone wants to go
 void update_waypoint(uint8_t wp_id, struct FloatVect3 * target_ned) {
 
@@ -127,11 +159,39 @@ void follow_diagonal_approach(void) {
     return;
   }
   target_get_vel(&target_vel);
+
+  // filter target values
+  update_butterworth_2_low_pass(&target_pos_filt[0], target_pos.x);
+  update_butterworth_2_low_pass(&target_pos_filt[1], target_pos.y);
+  update_butterworth_2_low_pass(&target_pos_filt[2], target_pos.z);
+
+  update_butterworth_2_low_pass(&target_vel_filt[0], target_vel.x);
+  update_butterworth_2_low_pass(&target_vel_filt[1], target_vel.y);
+  update_butterworth_2_low_pass(&target_vel_filt[2], target_vel.z);
+
+  // just filtering the heading will go wrong if it wraps, instead do:
+  // take the diff with current heading_filtered, wrap that, add to current, insert in filter and wrap again?
+  // example: target_heading - target_heading_filt (170 - -170 = 340), -> -20 -> -190 -> into filter -> wrap
+  float heading_diff = target_heading - target_heading_filt.o[0];
+  FLOAT_ANGLE_NORMALIZE(heading_diff);
+  float target_heading_input = target_heading_filt.o[0] + heading_diff;
+  update_butterworth_2_low_pass(&target_heading_filt, target_heading_input);
+  float target_heading_filt_wrapped = target_heading_filt.o[0];
+  FLOAT_ANGLE_NORMALIZE(target_heading_filt_wrapped);
+
+  target_pos.x = target_pos_filt[0].o[0];
+  target_pos.y = target_pos_filt[1].o[0];
+  target_pos.z = target_pos_filt[2].o[0];
+
+  target_vel.x = target_vel_filt[0].o[0];
+  target_vel.y = target_vel_filt[1].o[0];
+  target_vel.z = target_vel_filt[2].o[0];
+
   VECT3_SMUL(target_vel, target_vel, amt.speed_gain);
 
   // Reference model
   float gamma_ref = RadOfDeg(amt.slope_ref);
-  float psi_ref = RadOfDeg(target_heading + amt.psi_ref);
+  float psi_ref = RadOfDeg(target_heading_filt_wrapped + amt.psi_ref);
 
   amt.rel_unit_vec.x = cosf(gamma_ref) * cosf(psi_ref);
   amt.rel_unit_vec.y = cosf(gamma_ref) * sinf(psi_ref);
diff --git a/sw/airborne/modules/ctrl/approach_moving_target.h b/sw/airborne/modules/ctrl/approach_moving_target.h
index 0c7097a064..f48be866ac 100644
--- a/sw/airborne/modules/ctrl/approach_moving_target.h
+++ b/sw/airborne/modules/ctrl/approach_moving_target.h
@@ -47,9 +47,11 @@ struct Amt {
 
 extern struct Amt amt;
 extern float amt_err_slowdown_gain;
+extern float cutoff_freq_filters_hz;
 
 extern void approach_moving_target_init(void);
 extern void follow_diagonal_approach(void);
 extern void approach_moving_target_enable(uint8_t wp_id);
+extern void approach_moving_target_set_low_pass_freq(float filter_freq);
 
 #endif // APPROACH_MOVING_TARGET_H