From 152c1507e089999906445720f75f156a4cf7b9d1 Mon Sep 17 00:00:00 2001
From: Samuel Tabor <samuelctabor@gmail.com>
Date: Fri, 29 Mar 2019 15:38:59 +0000
Subject: [PATCH] AP_Soaring: Make the EKF states the actual NE position of the
 thermal, rather than the position relative to aircraft.

---
 libraries/AP_Soaring/AP_Soaring.cpp           |  8 +++----
 libraries/AP_Soaring/ExtendedKalmanFilter.cpp | 21 +++++++++++--------
 libraries/AP_Soaring/ExtendedKalmanFilter.h   |  4 ++--
 3 files changed, 18 insertions(+), 15 deletions(-)

diff --git a/libraries/AP_Soaring/AP_Soaring.cpp b/libraries/AP_Soaring/AP_Soaring.cpp
index 7f8a40529d..3e3f3ea94a 100644
--- a/libraries/AP_Soaring/AP_Soaring.cpp
+++ b/libraries/AP_Soaring/AP_Soaring.cpp
@@ -136,7 +136,7 @@ SoaringController::SoaringController(AP_AHRS &ahrs, AP_SpdHgtControl &spdHgt, co
 
 void SoaringController::get_target(Location &wp)
 {
-    _ahrs.get_position(wp);
+    wp = _ahrs.get_home();
     wp.offset(_ekf.X[2], _ekf.X[3]);
 }
 
@@ -228,8 +228,8 @@ void SoaringController::init_thermalling()
     // New state vector filter will be reset. Thermal location is placed in front of a/c
     const float init_xr[4] = {INITIAL_THERMAL_STRENGTH,
                               INITIAL_THERMAL_RADIUS,
-                              thermal_distance_ahead * cosf(_ahrs.yaw),
-                              thermal_distance_ahead * sinf(_ahrs.yaw)};
+                              position.x + thermal_distance_ahead * cosf(_ahrs.yaw),
+                              position.y + thermal_distance_ahead * sinf(_ahrs.yaw)};
     const VectorN<float,4> xr{init_xr};
 
     // Also reset covariance matrix p so filter is not affected by previous data
@@ -302,7 +302,7 @@ void SoaringController::update_thermalling()
                                            (double)dy_w);
 
     //log_data();
-    _ekf.update(_vario.reading,dx, dy);       // update the filter
+    _ekf.update(_vario.reading, current_position.x, current_position.y, dx_w, dy_w);       // update the filter
 
     _prev_update_location = current_position;      // save for next time
     _prev_update_time = AP_HAL::micros64();
diff --git a/libraries/AP_Soaring/ExtendedKalmanFilter.cpp b/libraries/AP_Soaring/ExtendedKalmanFilter.cpp
index 674ac93924..4a6bdc729c 100644
--- a/libraries/AP_Soaring/ExtendedKalmanFilter.cpp
+++ b/libraries/AP_Soaring/ExtendedKalmanFilter.cpp
@@ -2,20 +2,20 @@
 #include "AP_Math/matrixN.h"
 
 
-float ExtendedKalmanFilter::measurementpredandjacobian(VectorN<float,N> &A)
+float ExtendedKalmanFilter::measurementpredandjacobian(VectorN<float,N> &A, float Px, float Py)
 {
     // This function computes the Jacobian using equations from
     // analytical derivation of Gaussian updraft distribution
     // This expression gets used lots
-    float expon = expf(- (powf(X[2], 2) + powf(X[3], 2)) / powf(X[1], 2));
+    float expon = expf(- (powf(X[2]-Px, 2) + powf(X[3]-Py, 2)) / powf(X[1], 2));
     // Expected measurement
     float w = X[0] * expon;
 
     // Elements of the Jacobian
     A[0] = expon;
-    A[1] = 2 * X[0] * ((powf(X[2],2) + powf(X[3],2)) / powf(X[1],3)) * expon;
-    A[2] = -2 * (X[0] * X[2] / powf(X[1],2)) * expon;
-    A[3] = A[2] * X[3] / X[2];
+    A[1] = 2 * X[0] * ((powf(X[2]-Px,2) + powf(X[3]-Py,2)) / powf(X[1],3)) * expon;
+    A[2] = -2 * (X[0] * (X[2]-Px) / powf(X[1],2)) * expon;
+    A[3] = -2 * (X[0] * (X[3]-Py) / powf(X[1],2)) * expon;
     return w;
 }
 
@@ -29,7 +29,7 @@ void ExtendedKalmanFilter::reset(const VectorN<float,N> &x, const MatrixN<float,
 }
 
 
-void ExtendedKalmanFilter::update(float z, float Vx, float Vy)
+void ExtendedKalmanFilter::update(float z, float Px, float Py, float driftX, float driftY)
 {
     MatrixN<float,N> tempM;
     VectorN<float,N> H;
@@ -38,8 +38,8 @@ void ExtendedKalmanFilter::update(float z, float Vx, float Vy)
     
     // LINE 28
     // Estimate new state from old.
-    X[2] -= Vx;
-    X[3] -= Vy;
+    X[2] += driftX;
+    X[3] += driftY;
 
     // LINE 33
     // Update the covariance matrix
@@ -52,7 +52,7 @@ void ExtendedKalmanFilter::update(float z, float Vx, float Vy)
     // state
     // LINE 37
     // [z1,H] = ekf.jacobian_h(x1);
-    float z1 = measurementpredandjacobian(H);
+    float z1 = measurementpredandjacobian(H, Px, Py);
 
     // LINE 40
     // P12 = P * H';
@@ -68,6 +68,9 @@ void ExtendedKalmanFilter::update(float z, float Vx, float Vy)
     // X = x1 + K * (z - z1);
     X += K * (z - z1);
 
+    // Make sure X[1] stays positive.
+    X[1] = X[1]>0 ? X[1]: -X[1];
+
     // Correct the covariance too.
     // LINE 46
     // NB should be altered to reflect Stengel
diff --git a/libraries/AP_Soaring/ExtendedKalmanFilter.h b/libraries/AP_Soaring/ExtendedKalmanFilter.h
index 6775b7c08f..b50a85e41d 100644
--- a/libraries/AP_Soaring/ExtendedKalmanFilter.h
+++ b/libraries/AP_Soaring/ExtendedKalmanFilter.h
@@ -20,8 +20,8 @@ public:
     MatrixN<float,N> Q;
     float R;
     void reset(const VectorN<float,N> &x, const MatrixN<float,N> &p, const MatrixN<float,N> q, float r);
-    void update(float z, float Vx, float Vy);
+    void update(float z, float Px, float Py, float driftX, float driftY);
 
 private:
-    float measurementpredandjacobian(VectorN<float,N> &A);
+    float measurementpredandjacobian(VectorN<float,N> &A, float Px, float Py);
 };