Diff: VerticalEstimator/VerticalEstimator.cpp

Revision:

9:15058b4fa090

Parent:

7:220ce3839be8

Child:

11:fad579538b4c

--- a/VerticalEstimator/VerticalEstimator.cpp	Thu Sep 27 18:55:22 2018 +0000
+++ b/VerticalEstimator/VerticalEstimator.cpp	Fri Sep 28 15:08:41 2018 +0000
@@ -9,16 +9,30 @@
 // Initialize class 
 void VerticalEstimator::init()
 {
-    // Initialize range sensor object
     range.init();
 }
 
-//
+// Estimate vertical position and velocity
 void VerticalEstimator::estimate(float phi, float theta)
 {
-    // 
     range.read();
-    float z_new = range.z*cos(phi)*cos(theta);
-    w = (z_new-z)/dt_pos;
-    z = z_new;
+    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