diff --git a/libraries/AP_Soaring/AP_Soaring.h b/libraries/AP_Soaring/AP_Soaring.h
index a00e905932..c1a2084e43 100644
--- a/libraries/AP_Soaring/AP_Soaring.h
+++ b/libraries/AP_Soaring/AP_Soaring.h
@@ -17,10 +17,10 @@
 #include <AP_SpdHgtControl/AP_SpdHgtControl.h>
 
 #define INITIAL_THERMAL_STRENGTH 2.0
-#define INITIAL_THERMAL_RADIUS 30.0 //150.0
+#define INITIAL_THERMAL_RADIUS 80.0
 #define INITIAL_STRENGTH_COVARIANCE 0.0049
-#define INITIAL_RADIUS_COVARIANCE 2500.0
-#define INITIAL_POSITION_COVARIANCE 300.0
+#define INITIAL_RADIUS_COVARIANCE 400.0
+#define INITIAL_POSITION_COVARIANCE 400.0
 
 
 class SoaringController {
diff --git a/libraries/AP_Soaring/ExtendedKalmanFilter.cpp b/libraries/AP_Soaring/ExtendedKalmanFilter.cpp
index 4a6bdc729c..f0377164aa 100644
--- a/libraries/AP_Soaring/ExtendedKalmanFilter.cpp
+++ b/libraries/AP_Soaring/ExtendedKalmanFilter.cpp
@@ -69,7 +69,7 @@ void ExtendedKalmanFilter::update(float z, float Px, float Py, float driftX, flo
     X += K * (z - z1);
 
     // Make sure X[1] stays positive.
-    X[1] = X[1]>0 ? X[1]: -X[1];
+    X[1] = X[1]>40.0 ? X[1]: 40.0;
 
     // Correct the covariance too.
     // LINE 46