wangine
Joined: 07 Jul 2009
Posts: 98
Location: Curtea de Arges, Romania
|
| low pass and high pass filter |
 Posted: Sat Nov 26, 2011 2:17 pm |
 |
|
Simple low pass and high pass filter.
The magic number is the coeficients (fill_rate_hp & fill_rate_lp).
| Code: | /********************************************************************/
/* Writer Tanase Bogdan */
/* (c) copyright www.hexhoby.ro */
/* ALL RIGHTS RESERVED */
/* */
/********************************************************************/
/* Ver 0.2 release 28/august/2011 */
/********************************************************************/
float32 fill_rate_lp = 0.9689f; //float32 fill_rate = 0.8958700000f;
float32 fill_rate_hp = 0.9921767002f;
float32 low_pass = 0;
float32 high_pass = 0;
struct {
float lp_x_0;
float lp_x_1;
float lp_y_0;
float hp_x_0;
float hp_x_1;
float hp_y_0;
}filtru;
//~~~~~~~~~~~~~~~~~~~~~~~init lp_filter~~~~~~~~~~~~~~~~~~~~~~~~~~~
void init_lp_filter()
{
filtru.lp_x_0 = 0.0004171f;
filtru.lp_x_1 = 0.0004227f;
filtru.lp_y_0 = 0.0196256f;
}
//~~~~~~~~~~~~~~~~~~~lowpass filter angle from accelerometer ~~~~~~~~~~~~~~~~
float32 lp_filter(float32 lp_f)
{
filtru.lp_x_0 = filtru.lp_x_1;
filtru.lp_x_1 = lp_f * 0.00391165f; //(1.0f/2.556465999e+02f)>> //0.00391164991199243405231770500852
low_pass = filtru.lp_y_0 = filtru.lp_x_0 + filtru.lp_x_1 + fill_rate_lp * filtru.lp_y_0;
return low_pass ;
}
//~~~~~~~~~~~~~~~~~~~~~highpass filter angle rate from gyro ~~~~~~~~not necesary (kalman) ha
float hp_filter(float32 hp_f)
{
filtru.hp_x_0 = filtru.hp_x_1;
filtru.hp_x_1 = hp_f * 0.9960883500f; //0.99608835009221601668808968822535
filtru.hp_y_0 = filtru.hp_x_1 - filtru.hp_x_0 +
fill_rate_hp * filtru.hp_y_0;
high_pass = filtru.hp_y_0;
return high_pass;
}
//========================================== |
and how it works
http://www.youtube.com/watch?v=Y6e-i7dk23w
The blue line is the filter pass. |
|
FAQ 
Search
Register
Profile 
Log in to check your private messages 
Log in
