Diff: VerticalEstimator/VerticalEstimator.cpp

Revision:

11:fad579538b4c

Parent:

9:15058b4fa090

Child:

13:1a871ebd35bb

--- a/VerticalEstimator/VerticalEstimator.cpp	Fri Sep 28 15:40:17 2018 +0000
+++ b/VerticalEstimator/VerticalEstimator.cpp	Fri Sep 28 18:52:33 2018 +0000
@@ -12,27 +12,22 @@
     range.init();
 }
 
-// Estimate vertical position and velocity
-void VerticalEstimator::estimate(float phi, float theta)
-{
-    range.read();
-    measure(phi,theta);
-    predict();
-    z = rho_ver*z_m+(1-rho_ver)*z_p;
-    w = rho_ver*w_m+(1-rho_ver)*w_p;
-}
-
-// Measure vertical position and velocity
-void VerticalEstimator::measure(float phi, float theta)
-{ 
-    float z_m_new = range.z*cos(phi)*cos(theta);
-    w_m = (z_m_new-z_m)/dt_pos;
-    z_m = z_m_new;
-}
-
 // Predict vertical position and velocity
 void VerticalEstimator::predict()
 {
-    z_p = z+w*dt_pos;
-    w_p = w;
-}
\ No newline at end of file
+    z = z+w*dt_att;
+    w = w;
+}
+
+// Update vertical position and velocity by merging range sensor readings
+void VerticalEstimator::update(float phi, float theta)
+{
+    range.read();
+    float z_m = range.z*cos(phi)*cos(theta);
+    float w_m = (z_m-z_m_last)/dt_pos;
+    z = (1-rho_ver)*z+rho_ver*z_m;
+    w = (1-rho_ver)*w+rho_ver*w_m;
+    z_m_last = z_m;
+}
+
+