AP_Soaring: Adjust initial EKF values and limit R to 40.0m.

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 {

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