HorizontalEstimator.cpp

00001 #include "mbed.h"
00002 #include "HorizontalEstimator.h"
00003 
00004 // Class constructor
00005 HorizontalEstimator::HorizontalEstimator() : flow(PA_7,PA_6,PA_5,PB_4)
00006 {
00007     x = 0.0f;
00008     y = 0.0f;
00009     u = 0.0f;
00010     v = 0.0f;
00011     x_m_last = 0.0f;
00012     y_m_last = 0.0f;
00013 }
00014 
00015 // Initialize class 
00016 void HorizontalEstimator::init()
00017 {
00018     flow.init();
00019 }
00020 
00021 // Predict horizontal velocity from model
00022 void HorizontalEstimator::predict()
00023 {
00024     x = x+u*dt;
00025     y = y+v*dt;
00026     u = u;
00027     v = v;
00028 }
00029 
00030 // Correct horizontal velocity with measurements
00031 void HorizontalEstimator::correct(float phi, float theta, float p, float q, float z)
00032 {
00033     float div = (cos(phi)*cos(theta));
00034     if (div>0.5f)
00035     {
00036         flow.read();
00037         float d = z/div;
00038         float u_m = (flow.px*sigma+q)*d;
00039         float v_m = (flow.py*sigma-p)*d;
00040         float x_m = x_m_last + u_m*dt_flow;
00041         float y_m = y_m_last + v_m*dt_flow;
00042         x = (1-rho_hor)*x+rho_hor*x_m;
00043         y = (1-rho_hor)*y+rho_hor*y_m;
00044         u = (1-rho_hor)*u+rho_hor*u_m;
00045         v = (1-rho_hor)*v+rho_hor*v_m;
00046         x_m_last = x_m;
00047         y_m_last = y_m;
00048     }
00049 }