滤波算法

滤波是传感器处理中的重要算法，经常接触底层常常用到，以下总结了一些滤波算法，供以后参考调用。

下文分为三部分 

1、低通滤波

2、高通滤波

3、融合滤波

一、低通滤波

1.1RC滤波的数字低通滤波

　　指在截止频率fc的时候，增益为-3db(Aup=0.707)的滤波器，也是模电书中出现的第一种硬件滤波器，以下是对应的软件形式的1阶RC滤波器的数字形式(本断程序节选自匿名4轴)

　　一阶形式：Y(n)=(1-a)*Y(n-1)+a*X(n)

　　下式中 oldData表示上一次的输出Y(n-1)  newData表示新的输入X(n)

　　float LopPassFilter_RC_1st(float oldData, float newData, float a)
　　{
　　　　return oldData * (1 - a) + newData * a;
　　}

　　计算比例系数a:

　　float LopPassFilter_RC_1st_Factor_Cal(float deltaT, float Fcut)
　　{
　　　　return deltaT / (deltaT + 1 / (2 * M_PI * Fcut));
　　}

1.2均值滤波：

　　把一段时间内的数据累加后求平均值，达到平滑的作用，适用性广泛，元素越多滤波效果越好时延越高。

　　uint16_t LowPassFilter_Average(uint16_t data[],uint16_t length)

　　{

　　　　uint32_t add=0;
　　　　uint16_t result;
　　　　int i;

　　　　for(i=0;i<length;i++)
　　　　{
　　　　　　add += data[i];
　　　　}
　　　　result=add/length;
　　　　return result;
　　}

　　data[]放入一段时间里的数值，length：data数组的长度

1.3滑动滤波

　　在均值滤波的基础上，加上比例系数，最新的数据具有更大的比例，增加时效性。

　　以下列举了一个三个元素的滑动滤波

　　double LowPassFilter_Silding(double dataNewest,double dataMiddle,double dataLast)
　　{

　　　　#define PROPORTIONNEW 0.55

　　　　#define PROPORTIONMID 0.35

　　　　#define PROPORTIONLAST (1- PROPORTIONNEW -PROPORTIONMID )

　　　　double result;
　　　　result = PROPORTIONNEW *dataNewest+