From 42f5b5a2325817feb2ca1e73d798975d27f01470 Mon Sep 17 00:00:00 2001
From: Andrey Kolobov <akolobov@microsoft.com>
Date: Wed, 1 Mar 2017 15:12:12 +1100
Subject: [PATCH] AP_Soaring: fixes for matrixN changes

---
 libraries/AP_Soaring/AP_Soaring.cpp           | 4 ++--
 libraries/AP_Soaring/ExtendedKalmanFilter.cpp | 7 +++----
 libraries/AP_Soaring/ExtendedKalmanFilter.h   | 6 +++---
 3 files changed, 8 insertions(+), 9 deletions(-)

diff --git a/libraries/AP_Soaring/AP_Soaring.cpp b/libraries/AP_Soaring/AP_Soaring.cpp
index 5dcf3d67d0..aed83c3b98 100644
--- a/libraries/AP_Soaring/AP_Soaring.cpp
+++ b/libraries/AP_Soaring/AP_Soaring.cpp
@@ -208,9 +208,9 @@ void SoaringController::init_thermalling()
     float cov_q1 = powf(thermal_q1, 2); // State covariance
     float cov_q2 = powf(thermal_q2, 2); // State covariance
     const float init_q[4] = {cov_q1, cov_q2, cov_q2, cov_q2};
-    const MatrixN<4> q{init_q};
+    const MatrixN<float,4> q{init_q};
     const float init_p[4] = {INITIAL_STRENGTH_COVARIANCE, INITIAL_RADIUS_COVARIANCE, INITIAL_POSITION_COVARIANCE, INITIAL_POSITION_COVARIANCE};
-    const MatrixN<4> p{init_p};
+    const MatrixN<float,4> p{init_p};
 
     // New state vector filter will be reset. Thermal location is placed in front of a/c
     const float init_xr[4] = {INITIAL_THERMAL_STRENGTH,
diff --git a/libraries/AP_Soaring/ExtendedKalmanFilter.cpp b/libraries/AP_Soaring/ExtendedKalmanFilter.cpp
index 6cd6a78bb7..674ac93924 100644
--- a/libraries/AP_Soaring/ExtendedKalmanFilter.cpp
+++ b/libraries/AP_Soaring/ExtendedKalmanFilter.cpp
@@ -20,7 +20,7 @@ float ExtendedKalmanFilter::measurementpredandjacobian(VectorN<float,N> &A)
 }
 
 
-void ExtendedKalmanFilter::reset(const VectorN<float,N> &x, const MatrixN<N> &p, const MatrixN<N> q, float r)
+void ExtendedKalmanFilter::reset(const VectorN<float,N> &x, const MatrixN<float,N> &p, const MatrixN<float,N> q, float r)
 {
     P = p;
     X = x;
@@ -31,8 +31,7 @@ void ExtendedKalmanFilter::reset(const VectorN<float,N> &x, const MatrixN<N> &p,
 
 void ExtendedKalmanFilter::update(float z, float Vx, float Vy)
 {
-    MatrixN<N> tempM;
-    VectorN<float,N> tempV;
+    MatrixN<float,N> tempM;
     VectorN<float,N> H;
     VectorN<float,N> P12;
     VectorN<float,N> K;
@@ -57,7 +56,7 @@ void ExtendedKalmanFilter::update(float z, float Vx, float Vy)
 
     // LINE 40
     // P12 = P * H';
-    P.mult(H, P12); //cross covariance    
+    P12.mult(P, H); //cross covariance 
     
     // LINE 41
     // Calculate the KALMAN GAIN
diff --git a/libraries/AP_Soaring/ExtendedKalmanFilter.h b/libraries/AP_Soaring/ExtendedKalmanFilter.h
index a9f1b7776d..0e01a213d3 100644
--- a/libraries/AP_Soaring/ExtendedKalmanFilter.h
+++ b/libraries/AP_Soaring/ExtendedKalmanFilter.h
@@ -16,10 +16,10 @@ public:
     ExtendedKalmanFilter(void) {}
     
     VectorN<float,N> X;
-    MatrixN<N> P;
-    MatrixN<N> Q;
+    MatrixN<float,N> P;
+    MatrixN<float,N> Q;
     float R;
-    void reset(const VectorN<float,N> &x, const MatrixN<N> &p, const MatrixN<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);
 
 private: