//=======================================
// Recursive complimentary filter
// Transition frequency at 0.25% 
// nyquist frequency.
//---------------------------------------
// Copyright 2008 Matthias Toussaint
// Usable under the terms of the GPL 2.0
//=======================================
// Filter 'object' definition
//
typedef struct 
{
 float hp_x0, hp_x1;
 float hp_y0;
 float lp_x0, lp_x1;
 float lp_y0;
 float hp_last;
 float hp_int;
} CompFilter;
// very simple complimentary filter 
// (0.25% nyquist hp-lp combination)
//
float comp_filter_step( CompFilter *filter, 
 float angle, 
 float rate );

float comp_filter_step( CompFilter *filter, 
 float raw_angle, 
 float raw_angle_rate )
{
 // highpass filter angle rate from gyro
 //
 filter->hp_x0 = filter->hp_x1;
 filter->hp_x1 = raw_angle_rate * (1.0f/1.003927011f);
 filter->hp_y0 = filter->hp_x1 - filter->hp_x0 + 
 0.9921767002f*filter->hp_y0;
 
 // integrate angle rate using trapezoidal rule
 //
 filter->hp_int -= 0.5f*(filter->hp_last + 
 filter->hp_y0);
 filter->hp_last = filter->hp_y0;
 
 // lowpass filter angle from accelerometer
 //
 filter->lp_x0 = filter->lp_x1;
 filter->lp_x1 = raw_angle * (1.0f/2.556465999e+02f);
 float lp = filter->lp_y0 = filter->lp_x0 + 
 filter->lp_x1 + 
 0.9921767002f*filter->lp_y0;
 
 return lp + filter->hp_int;
}

//=======================================
// Savitzky-Golay 1st derivation filter
//---------------------------------------
// Copyright 2008 Matthias Toussaint
// Usable under the terms of the GPL 2.0
//=======================================
// Filter 'object' definition
//
typedef struct 
{
 float x0, x1, x2, x3, x4, x5, x6;
} SavgolFilter7;
float sg_derive7_step( SavgolFilter7 *filter, 
 float angle );

float sg_derive7_step( SavgolFilter7 *filter, 
 float angle )
{
 filter->x0 = filter->x1;
 filter->x1 = filter->x2;
 filter->x2 = filter->x3;
 filter->x3 = filter->x4;
 filter->x4 = filter->x5;
 filter->x5 = filter->x6;
 filter->x6 = value;
 
 return 0.1071429f*(filter->x6-filter->x0) + 
 0.0714286f*(filter->x5-filter->x1) + 
 0.0357143f*(filter->x4-filter->x2); 
}