Kalman_V_A_Aoff_observer.cpp

Go to the documentation of this file.

/***********************************************************************/
#include <Kalman_V_A_Aoff_observer.h>
 
ROM float Kalman_V_A_Aoff_observer_t::Gain[N][L]= 
    {
       0.045525746461218f,   0.005179336297606f,
       0.102816402740272f,   0.906791781469036f,
      -0.097637066442666f,   0.001591461803718f
    };
 
void Kalman_V_A_Aoff_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 x_est_2 = x[2];
 
  float innovation_v = velocity     - x_est_0;
  float innovation_a = acceleration - x_est_1 - x_est_2;
 
  // 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;
  x[2] = x_est_2 + Gain[2][0] * innovation_v + Gain[2][1] * innovation_a;
}