From fdf7eae01cf463b0ebd93555472739404670d99d Mon Sep 17 00:00:00 2001 From: Samuel Tabor Date: Sun, 7 Jul 2019 13:10:22 +0100 Subject: [PATCH] AP_Soaring: Adjust initial EKF values and limit R to 40.0m. --- libraries/AP_Soaring/AP_Soaring.h | 6 +++--- libraries/AP_Soaring/ExtendedKalmanFilter.cpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) 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 #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