diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
index 5008c53cd4..6e5df1a027 100644
--- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
+++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
@@ -403,8 +403,8 @@ FixedwingPositionINDIControl::_read_trajectory_coeffs_csv(char *filename)
     // =======================================================================
     bool error = false;
 
-    //char home_dir[200] = "/home/marvin/Documents/master_thesis_ADS/PX4/Git/ethzasl_fw_px4/src/modules/fw_dyn_soar_control/trajectories/";
-    char home_dir[200] = PX4_ROOTFSDIR"/fs/microsd/trajectories/";
+    char home_dir[200] = "/home/marvin/Documents/master_thesis_ADS/PX4/Git/ethzasl_fw_px4/src/modules/fw_dyn_soar_control/trajectories/";
+    //char home_dir[200] = PX4_ROOTFSDIR"/fs/microsd/trajectories/";
     //PX4_ERR(home_dir);
     strcat(home_dir,filename);
     FILE* fp = fopen(home_dir, "r");
@@ -591,8 +591,12 @@ FixedwingPositionINDIControl::Run()
         // compute expected AoA from g-forces:
         Vector3f body_force = _mass*R_bi*(_acc + Vector3f{0.f,0.f,9.81f});
         // approximate lift force, since implicit equation cannot be solved analytically:
-        float lift = -body_force(2);
-        float AoA_approx = ((2.f*lift)/(_rho*_area*(fmaxf(_airspeed*_airspeed,_stall_speed*_stall_speed))+0.001f) - _C_L0)/_C_L1;
+        // since alpha<<1, we approximate the lift force L = sin(alpha)*Fx - cos(alpha)*Fz
+        // as L = alpha*Fx - Fz
+        float Fx = body_force(0);
+        float Fz = -body_force(2);
+        float AoA_approx = (((2.f*Fz)/(_rho*_area*(fmaxf(_airspeed*_airspeed,_stall_speed*_stall_speed))+0.001f) - _C_L0)/_C_L1) / 
+                            (1 - ((2.f*Fx)/(_rho*_area*(fmaxf(_airspeed*_airspeed,_stall_speed*_stall_speed))+0.001f)/_C_L1));
         AoA_approx = constrain(AoA_approx,-0.2f,0.2f);
         Vector3f vel_air = R_ib*(Vector3f{_airspeed,0.f,tanf(AoA_approx)*_airspeed});
         Vector3f wind = _vel - vel_air;
@@ -600,7 +604,7 @@ FixedwingPositionINDIControl::Run()
         wind(1) = _lp_filter_wind[1].apply(wind(1));
         wind(2) = _lp_filter_wind[2].apply(wind(2));
         _set_wind_estimate(wind);
-        //PX4_INFO("wind estimate:\t%.4f\t%.4f\t%.4f", (double)_wind_estimate(0),(double)_wind_estimate(1),(double)_wind_estimate(2));
+        PX4_INFO("wind estimate:\t%.4f\t%.4f\t%.4f", (double)_wind_estimate(0),(double)_wind_estimate(1),(double)_wind_estimate(2));
 
 
         // only run actuators poll, when our module is not publishing:
diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp
index 92c5eff683..2c4bb602d2 100644
--- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp
+++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp
@@ -296,7 +296,7 @@ private:
 	// controller frequency
 	const float _sample_frequency = 200.f;
 	// Low-Pass filters stage 1
-	const float _cutoff_frequency_1 = 5.f;
+	const float _cutoff_frequency_1 = 30.f;
 	math::LowPassFilter2p _lp_filter_accel[3] {{_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}};	// linear acceleration
 	math::LowPassFilter2p _lp_filter_force[3] {{_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}};	// force command
 	math::LowPassFilter2p _lp_filter_omega[3] {{_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}};	// body rates
diff --git a/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c b/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c
index 15f184800c..8e7c903a5a 100644
--- a/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c
+++ b/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c
@@ -243,7 +243,7 @@ PARAM_DEFINE_FLOAT(DS_K_X_YAW, 1.0f);
  * @increment 0.1
  * @group FW DYN SOAR Control
  */
-PARAM_DEFINE_FLOAT(DS_K_V_ROLL, 1.3f);
+PARAM_DEFINE_FLOAT(DS_K_V_ROLL, 1.5f);
 
 /**
  * pitch gain of K_v (velocity error gain)
@@ -255,7 +255,7 @@ PARAM_DEFINE_FLOAT(DS_K_V_ROLL, 1.3f);
  * @increment 0.1
  * @group FW DYN SOAR Control
  */
-PARAM_DEFINE_FLOAT(DS_K_V_PITCH, 1.3f);
+PARAM_DEFINE_FLOAT(DS_K_V_PITCH, 1.5f);
 
 /**
  * yaw gain of K_v (velocity error gain)
@@ -267,7 +267,7 @@ PARAM_DEFINE_FLOAT(DS_K_V_PITCH, 1.3f);
  * @increment 0.1
  * @group FW DYN SOAR Control
  */
-PARAM_DEFINE_FLOAT(DS_K_V_YAW, 1.3f);
+PARAM_DEFINE_FLOAT(DS_K_V_YAW, 1.5f);
 
 /**
  * roll gain of K_A (acceleration error gain)
@@ -391,7 +391,7 @@ PARAM_DEFINE_FLOAT(DS_K_W_YAW, 1.0f);
  * @increment 0.001
  * @group FW DYN SOAR Control
  */
-PARAM_DEFINE_FLOAT(DS_K_ACT_ROLL, 0.2f);
+PARAM_DEFINE_FLOAT(DS_K_ACT_ROLL, 0.25f);
 
 /**
  * pitch gain of K_ACT (actuator deflection gain)