滤波是传感器处理中的重要算法,经常接触底层常常用到,以下总结了一些滤波算法,供以后参考调用。
下文分为三部分
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+