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 // Initialize class
00015 void HorizontalEstimator::init()
00016 {
00017     flow.init();
00018 }
00019 // Predict horizontal position and velocity from model
00020 void HorizontalEstimator::predict()
00021 {
00022     x = x + u*dt;
00023     y = y + v*dt;
00024 }
00025 // Correct horizontal position and velocity with measurements
00026 void HorizontalEstimator::correct(float phi,float theta,float p,float q,float z)
00027 {
00028     float Cphi = cos(phi);
00029     float Ctheta = cos(theta);
00030     
00031     if( Cphi >= 0.7f && Ctheta >= 0.7f )
00032     {
00033         float d = z/(Cphi*Ctheta);
00034         
00035         flow.read();
00036         float u_m = (sigma*flow.x + q)*d;
00037         float v_m = (sigma*flow.y - p)*d;
00038         
00039         float x_m = x_m_last + u_m*dt_flow;
00040         float y_m = y_m_last + v_m*dt_flow;
00041         
00042         x = (1.0f - rho_horiz_corr)*x + rho_horiz_corr*x_m;
00043         y = (1.0f - rho_horiz_corr)*y + rho_horiz_corr*y_m;
00044         u = (1.0f - rho_horiz_corr)*u + rho_horiz_corr*u_m;
00045         v = (1.0f - rho_horiz_corr)*v + rho_horiz_corr*v_m;
00046         
00047         x_m_last = x_m;
00048         y_m_last = y_m;
00049     }
00050 }