Diff: HorizontaEstimator/HorizontalEstimator.cpp

Revision:

12:9fed6f656f88

Parent:

10:7074bc7038d6

Child:

15:155ca63b7884

--- a/HorizontaEstimator/HorizontalEstimator.cpp	Fri Sep 28 18:52:33 2018 +0000
+++ b/HorizontaEstimator/HorizontalEstimator.cpp	Fri Sep 28 21:45:50 2018 +0000
@@ -13,11 +13,21 @@
     flow.init();
 }
 
+// Predict vertical position and velocity
+void HorizontalEstimator::predict()
+{
+    u = u;
+    v = v;
+}
+
 //
-void HorizontalEstimator::estimate(float p, float q, float z)
+void HorizontalEstimator::update(float phi, float theta, float p, float q, float z)
 {
     // 
     flow.read();
-    u = 0.5f*u+0.5f*(flow.x-p)*z;
-    v = 0.5f*v+0.5f*(flow.y-q)*z;
+    float d = z/(cos(phi)*cos(theta));
+    float u_m = (flow.x+q)*d;
+    float v_m = (flow.y-p)*d;
+    u = (1-rho_hor)*u+rho_hor*u_m;
+    v = (1-rho_hor)*v+rho_hor*v_m;
 }
\ No newline at end of file