Kalman_V_A_observer.cpp

Go to the documentation of this file.

/***********************************************************************/
#include <Kalman_V_A_observer.h>
 
ROM float Kalman_V_A_observer_t::Gain[N][L]= 
    {
       0.002460426151335f,   0.008147862943514f,
       0.000527203519767f,   0.011290372131367f,
    };
 
float Kalman_V_A_observer_t::update( const float velocity, const float acceleration)
{
  // predict x[] by propagating it through the system model
  float x_est_0 = x[0] + Ta * x[1];
  float x_est_1 = x[1];
 
  float innovation_v = velocity - x_est_0;
  float innovation_a = acceleration - x_est_1;
 
  // x[] correction
  x[0] = x_est_0 + Gain[0][0] * innovation_v + Gain[0][1] * innovation_a;
  x[1] = x_est_1 + Gain[1][0] * innovation_v + Gain[1][1] * innovation_a;
 
  return x[1]; // return velocity
}