diff --git a/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp b/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp
index 6ff6a68d81..cbc478aa8f 100644
--- a/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp
+++ b/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp
@@ -201,8 +201,6 @@ FixedwingShearEstimator::perform_posterior_update(float height, Vector3f wind)
     // first fill the horizontal observation matrix:
     float vx = _X_prior_horizontal(0);
     float vy = _X_prior_horizontal(1);
-    //float bx = _X_prior_horizontal(1);
-    //float by = _X_prior_horizontal(3);
     float h = _X_prior_horizontal(4);
     float a = _X_prior_horizontal(5);
 
@@ -262,7 +260,6 @@ FixedwingShearEstimator::perform_posterior_update(float height, Vector3f wind)
         Matrix<float, 6, 6> identity_1;
         z_expected_horizontal(0) = _X_prior_horizontal(0)/(1.f+expf(-_X_prior_horizontal(5)*(_current_height-_X_prior_horizontal(4)))) + _X_prior_horizontal(2);
         z_expected_horizontal(1) = _X_prior_horizontal(1)/(1.f+expf(-_X_prior_horizontal(5)*(_current_height-_X_prior_horizontal(4)))) + _X_prior_horizontal(3);
-        z_expected_horizontal.print();
         wind_horizontal(0) = _current_wind(0);
         wind_horizontal(1) = _current_wind(1);
         identity_1.setIdentity();
@@ -335,11 +332,9 @@ FixedwingShearEstimator::Run()
             // posterior update
             perform_posterior_update(_current_height, _current_wind);
 
-
-            
             // check if filter diverges 
             // maybe reset filters...
-            if (sqrtf(_P_posterior_horizontal(4,4))>=10.f) {
+            if (sqrtf(_P_posterior_horizontal(4,4))>=6.f||sqrtf(_P_posterior_horizontal(5,5))>=0.2f) {
                 PX4_WARN("large height uncertainty, resetting filter");
                 reset_filter();
             }
diff --git a/src/modules/fw_dyn_soar_estimator/fw_dyn_soar_estimator_params.c b/src/modules/fw_dyn_soar_estimator/fw_dyn_soar_estimator_params.c
index 58fe7a52eb..cfdd76f2a1 100644
--- a/src/modules/fw_dyn_soar_estimator/fw_dyn_soar_estimator_params.c
+++ b/src/modules/fw_dyn_soar_estimator/fw_dyn_soar_estimator_params.c
@@ -22,7 +22,7 @@
  * @increment 0.000001
  * @group FW DYN SOAR Control
  */
-PARAM_DEFINE_FLOAT(DS_SIGMA_Q_V, 0.01f);
+PARAM_DEFINE_FLOAT(DS_SIGMA_Q_V, 0.3f);
 
 /**
  * Standard deviation of vertical shear position
@@ -36,7 +36,7 @@ PARAM_DEFINE_FLOAT(DS_SIGMA_Q_V, 0.01f);
  * @increment 0.000001
  * @group FW DYN SOAR Control
  */
-PARAM_DEFINE_FLOAT(DS_SIGMA_Q_H, 0.1f);
+PARAM_DEFINE_FLOAT(DS_SIGMA_Q_H, 0.3f);
 
 /**
  * Standard deviation of velicity state in shear model
@@ -50,7 +50,7 @@ PARAM_DEFINE_FLOAT(DS_SIGMA_Q_H, 0.1f);
  * @increment 0.000001
  * @group FW DYN SOAR Control
  */
-PARAM_DEFINE_FLOAT(DS_SIGMA_Q_A, 0.003f);
+PARAM_DEFINE_FLOAT(DS_SIGMA_Q_A, 0.03f);
 
 /**
  * Standard deviation of velicity measurement (wind)
@@ -64,7 +64,7 @@ PARAM_DEFINE_FLOAT(DS_SIGMA_Q_A, 0.003f);
  * @increment 0.000001
  * @group FW DYN SOAR Control
  */
-PARAM_DEFINE_FLOAT(DS_SIGMA_R_V, 4.f);
+PARAM_DEFINE_FLOAT(DS_SIGMA_R_V, 3.f);
 
 /**
  * Initial guess  of the vertical shear position
@@ -78,4 +78,4 @@ PARAM_DEFINE_FLOAT(DS_SIGMA_R_V, 4.f);
  * @increment 0.1
  * @group FW DYN SOAR Control
  */
-PARAM_DEFINE_FLOAT(DS_INIT_H, 100.f);
\ No newline at end of file
+PARAM_DEFINE_FLOAT(DS_INIT_H, 110.f);
\ No newline at end of file