KalmanVario.cpp

Go to the documentation of this file.

/***********************************************************************/
#include <KalmanVario.h>
 
ROM float KalmanVario_t::Gain[N][L]= 
    {
       0.022706480781195f,   0.000238300640696f,
       0.026080120255934f,   0.008557096024865f,
       0.012200483136450f,   0.282217429952530f,
      -0.011857330213848f,   0.000264240373951f
    };
 
float KalmanVario_t::update( const float altitude, const float acceleration)
{
  // predict x[] by propagating it through the system model
  float x_est_0 = x[0] + Ta * x[1] + Ta_s_2 * x[2];
  float x_est_1 = x[1] + Ta * x[2];
  float x_est_2 = x[2];
  float x_est_3 = x[3];
 
  float innovation_x = altitude     - x_est_0;
  float innovation_a = acceleration - x_est_2 - x_est_3;
 
  // x[] correction
  x[0] = x_est_0 + Gain[0][0] * innovation_x + Gain[0][1] * innovation_a;
  x[1] = x_est_1 + Gain[1][0] * innovation_x + Gain[1][1] * innovation_a;
  x[2] = x_est_2 + Gain[2][0] * innovation_x + Gain[2][1] * innovation_a;
  x[3] = x_est_3 + Gain[3][0] * innovation_x + Gain[3][1] * innovation_a;
 
  return x[1]; // return velocity
}