Diff: VerticalEstimator/VerticalEstimator.cpp

Revision:

5:b1f5ea192d12

Child:

6:7a447d4ae677

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/VerticalEstimator/VerticalEstimator.cpp	Wed Oct 17 12:13:33 2018 +0000
@@ -0,0 +1,31 @@
+#include "mbed.h"
+#include "VerticalEstimator.h"
+// Class constructor
+VerticalEstimator :: VerticalEstimator() : range (PB_7 , PB_6 )
+{
+    z=0;
+    w=0;
+    z_m_last=0;
+}
+// Initialize class
+void VerticalEstimator :: init()
+{
+    range.init();
+}
+// Predict vertical position and velocity from model
+void VerticalEstimator :: predict()
+{
+    z=z+0.05f*w;
+    w=w;
+}
+// Correct vertical position and velocity with measurement
+void VerticalEstimator :: correct ( float phi , float theta )
+{
+    p=0.5f;
+    range.read;
+    float z_m = range.z;
+    float w_m = (z-z_m)/0.05f;
+    z=(1-p)*z+p*z_m;
+    w=(1-p)*w+p*w_m;
+    z_m_last = z_m;
+}