Diff: VerticalEstimator/VerticalEstimator.cpp

Branch:

yuri

Revision:

8:c96125e9ac70

Parent:

6:7a447d4ae677

--- a/VerticalEstimator/VerticalEstimator.cpp	Wed Oct 17 12:18:29 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,31 +0,0 @@
-#include "mbed.h"
-#include "VerticalEstimator.h"
-// Class constructor hhh
-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;
-}