diff --git a/conf/airframes/Poine/beth.xml b/conf/airframes/Poine/beth.xml
index 592b548249..847168a6e2 100644
--- a/conf/airframes/Poine/beth.xml
+++ b/conf/airframes/Poine/beth.xml
@@ -225,7 +225,7 @@ main_overo.CFLAGS += -DOVERO_LINK_MSG_UP=AutopilotMessageBethUp -DOVERO_LINK_MSG
 main_overo.CFLAGS  += -DDOWNLINK -DDOWNLINK_TRANSPORT=UdpTransport
 main_overo.srcs    += $(SRC_FMS)/udp_transport2.c downlink.c
 main_overo.srcs    += $(SRC_FMS)/fms_network.c
-main_overo.LDFLAGS += -levent
+main_overo.LDFLAGS += -levent -lm
 
 main_overo.srcs    += $(SRC_BETH)/overo_gcs_com.c
 main_overo.srcs    += $(SRC_BETH)/overo_file_logger.c
diff --git a/conf/settings/settings_beth.xml b/conf/settings/settings_beth.xml
index a92e5b80d4..9ef63b4717 100644
--- a/conf/settings/settings_beth.xml
+++ b/conf/settings/settings_beth.xml
@@ -9,6 +9,9 @@
       <dl_setting var="controller.tilt_sp" min="-15" step="0.5" max="15" module="beth/overo_controller"  shortname="tilt_sp" unit="rad" alt_unit="deg" alt_unit_coef="57.29578">
       </dl_setting>
 
+      <dl_setting var="estimator.lp_coeff" min="0.01" step="0.01" max="1" module="beth/overo_estimator"  shortname="elev_lp_coeff">
+      </dl_setting>
+
       <dl_setting var="controller.armed" min="0" step="1" max="2" shortname ="mode"/>
 
     </dl_settings>
diff --git a/sw/airborne/beth/overo_controller.c b/sw/airborne/beth/overo_controller.c
index 028072b8b1..f0e214b240 100644
--- a/sw/airborne/beth/overo_controller.c
+++ b/sw/airborne/beth/overo_controller.c
@@ -82,6 +82,7 @@ void control_run(void) {
 
   controller.cmd_pitch =  controller.cmd_pitch_ff + controller.cmd_pitch_fb; 
   controller.cmd_thrust = controller.cmd_thrust_ff + controller.cmd_thrust_fb + thrust_constant;
+  controller.cmd_thrust = controller.cmd_thrust*(1/cos(estimator.elevation));
 
   //if (controller.cmd_thrust<0.) controller.cmd_thrust = 0;
   Bound(controller.cmd_thrust,0,100);
diff --git a/sw/airborne/beth/overo_estimator.c b/sw/airborne/beth/overo_estimator.c
index c8157f0c24..d5e197a5bb 100644
--- a/sw/airborne/beth/overo_estimator.c
+++ b/sw/airborne/beth/overo_estimator.c
@@ -1,11 +1,12 @@
 #include "overo_estimator.h"
 
 #include "booz/booz_imu.h"
+#include <math.h>
 
 struct OveroEstimator estimator;
 
 void estimator_init(void) {
-
+  estimator.lp_coeff = 0.9;
 }
 
 //zyx
@@ -20,11 +21,17 @@ void estimator_run(uint16_t tilt_measure, uint16_t elevation_measure, uint16_t a
 
 
   estimator.tilt = -(tilt_neutral - (int32_t)tilt_measure ) * tilt_scale;
+  Bound(estimator.tilt,-89,89);
+  //TODO: lp filter tilt_dot
   estimator.tilt_dot = RATE_FLOAT_OF_BFP(booz_imu.gyro.q);
 
-  //TODO: Add rotation compensation to remove tilt effect on elev/azi
   estimator.elevation = (elevation_neutral - (int32_t)elevation_measure ) * elevation_scale;
-  estimator.elevation_dot = RATE_FLOAT_OF_BFP(booz_imu.gyro.p);
+  Bound(estimator.elevation,-45,45);
+
+  //rotation compensation (mixing of two gyro values to generate a reading that reflects rate along beth axes
+  float rotated_elev_dot = RATE_FLOAT_OF_BFP(booz_imu.gyro.p)*cos(estimator.tilt)+RATE_FLOAT_OF_BFP(booz_imu.gyro.r)*sin(estimator.tilt);
+  //low pass filter -- should probably increase order and maybe move filtering to measured values.
+  estimator.elevation_dot = estimator.elevation_dot +estimator.lp_coeff*(rotated_elev_dot-estimator.elevation_dot);
 
   estimator.azimuth = (azimuth_neutral - (int32_t)azimuth_measure ) * azimuth_scale;
   estimator.azimuth_dot = RATE_FLOAT_OF_BFP(booz_imu.gyro.r);
diff --git a/sw/airborne/beth/overo_estimator.h b/sw/airborne/beth/overo_estimator.h
index edabaff3c5..b9bcf7fd7f 100644
--- a/sw/airborne/beth/overo_estimator.h
+++ b/sw/airborne/beth/overo_estimator.h
@@ -12,6 +12,8 @@ struct OveroEstimator {
   float azimuth_dot;
   float elevation_dot;
   float tilt_dot;
+
+  float lp_coeff;
 };