Diff: HorizontaEstimator/HorizontalEstimator.cpp

Revision:

18:60516a4cba27

Parent:

17:f682b4a5686d

Child:

19:83b357d6806e

--- a/HorizontaEstimator/HorizontalEstimator.cpp	Fri Oct 05 19:32:35 2018 +0000
+++ b/HorizontaEstimator/HorizontalEstimator.cpp	Wed Oct 10 10:48:52 2018 +0000
@@ -4,8 +4,12 @@
 // Class constructor
 HorizontalEstimator::HorizontalEstimator() : flow(PA_7,PA_6,PA_5,PB_4)
 {
+    x = 0.0f;
+    y = 0.0f;
     u = 0.0f;
     v = 0.0f;
+    x_m_last = 0.0f;
+    y_m_last = 0.0f;
 }
 
 // Initialize class 
@@ -17,21 +21,29 @@
 // Predict horizontal velocity from model
 void HorizontalEstimator::predict()
 {
+    x = x+u*dt;
+    y = y+v*dt;
     u = u;
     v = v;
 }
 
-// Update horizontal velocity with measurement
-void HorizontalEstimator::update(float phi, float theta, float p, float q, float z)
+// Correct horizontal velocity with measurement
+void HorizontalEstimator::correct(float phi, float theta, float p, float q, float z)
 {
-    flow.read();
     float div = (cos(phi)*cos(theta));
     if (div>0.5f)
     {
+        flow.read();
         float d = z/div;
         float u_m = (flow.x+q)*d;
         float v_m = (flow.y-p)*d;
+        float x_m = x_m_last + u_m*dt_flow;
+        float y_m = y_m_last + v_m*dt_flow;
+        x = (1-rho_hor)*x+rho_hor*x_m;
+        y = (1-rho_hor)*y+rho_hor*y_m;
         u = (1-rho_hor)*u+rho_hor*u_m;
         v = (1-rho_hor)*v+rho_hor*v_m;
+        x_m_last = x_m;
+        y_m_last = y_m;
     }
 }
\ No newline at end of file