常用的滤波算法

引用：https://www.cnblogs.com/Lxk0825/p/9908915.html

---

一、低通滤波

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

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+ PROPORTIONMID *dataMiddle+PROPORTIONLAST *dataLast;
　　　　return result;
　　}

　　三个输入参数为连续3个读出的数据，其中 dataNewest 为最新数据，对应比例系数PROPORTIONNEW ，另外两个同理。比例系数根据需要改变

1.4中值滤波

　　当传感器采集的数据存在毛刺时，为了提取其中有效的数据，采用中值滤波的算法，只保存数据大小在中间的数值。

uint16_t LowPassFilter_GetMiddleValue( uint16_t *pusBuffer, uint8_t ucLength )
　　{
　　　　uint8_t i;
　　　　uint16_t usMiddle,usMax,usCount = 0,usPoint;

　　　　while( usCount <= (ucLength/2) )
　　　　{
　　　　　　usPoint = 0;
　　　　　　usMax = pusBuffer[0];
　　　　　　for( i=1;i usMax ) 
　　　　　　　　{
　　　　　　　　　　usMax = pusBuffer[i];
　　　　　　　　　　usPoint = i;
　　　　　　　　} 
　　　　　　}
　　　　　　usMiddle = usMax;
　　　　　　pusBuffer[usPoint] = 0; 
　　　　　　usCount++; 
　　　　}
　　　　return usMiddle;
　　}

　　*pusBuffer:待改变的起始地址  ucLength:长度

　　特别注意：为了运行效率，这个方法中没有对数组进行备份，会直接操作到源 *data并改变元素的值。

　　这个快速中值算法原理是只排序最大或者最小值的一半，一半中的最后一个即为中间值

1.5去除极值的XX滤波

　　把中值滤波的优点结合给其他滤波，使其他滤波器也具有够剔除少量毛刺的能力。

　　以下例子结合的是均值滤波，即去除极值的均值滤波

int16_t LowPassFilter_RemoveExtremumAverage(int16_t data[],int16_t length)
　　{
　　　　#define EXTERMUM_NUMBER 1 //number of extermum
　　　　int32_t sum=0;
　　　　int16_t i;

　　　　if(length<=2*EXTERMUM_NUMBER)
　　　　{
　　　　　　return 0;//length shorter than 2*EXTERMUM_NUMBER, return err
　　　　}

　　　　//sort
　　　　BubbleSort_int16(data,length);
　　　　

　　　　//average filter
　　　　for(i=EXTERMUM_NUMBER;i

　　输入长度为length的数组data[]。EXTERMUM_NUMBER定义去除极值的个数（1表示去掉1个最大值和1个最小值）。先用排序法（本例使用下面给出的冒泡排序）对data数组进行排序，去除1个最大值和1个最小值后剩余求平均值。

　　这段代码可能出现两个错误

　　1、length比2倍的EXTERMUM_NUMBER短，本例用return 0一笔带过，实际需要在程序return 0处作处理

　　2、如果输入length过长会使32位的sum在累加过程溢出，如果length很长建议改变均值滤波的顺序在累加之前线进行除法

　　下面给出子程序冒泡排序

void BubbleSort_int16(int16_t *a,int len)
　　{
　　　　int i;
　　　　int j;
　　　　int mid;
　　　　for(i=0;i*(a+j+1))
　　　　　　　　{
　　　　　　　　　　mid=*(a+j);
　　　　　　　　　　*(a+j)=*(a+j+1);
　　　　　　　　　　*(a+j+1)=mid;
　　　　　　　　}
　　　　　　}
　　　　}
　　}

1.6巴特沃斯低通滤波

　　借助MATLAB使用数字滤波的方法来模拟巴特沃斯滤波器：

　　IIR滤波的基本结构 y(n)+a1*y(n-1)+a2*y(n-2)....ai*y(n-i)=b0*x(n)+b1*x(n-1)+b2*x(n-2)....+bi*x(n-i);

　　这里简单把IIR滤波理解为滑动滤波的进阶形式，其中i为阶数，x(n)为本次输入，x(n-1)为上一次输入，如此类推x(n-i)为i次前的输入。y(n)为输出，y(n-1)为上一次的输出。y(n-i)为i次前的输出。 a和b分别为比例。

　　系数a和b改变达到模拟不同采样频率、不同截止频率的滤波效果。

　　以下使用matlab计算一个1000HZ采样频率，100HZ截止频率，2阶的巴特沃斯滤波器的IIR系数：

　　>>fdatool

1.7切比雪夫低通滤波

1.8卡尔曼滤波

卡尔曼滤波是一种预测型滤波，这里把卡尔曼滤波归纳到低通滤波中，是因为在适用过程中卡尔曼增益Kg会慢慢收敛，变成跟低通滤波一样的效果

float LowPassFilter_kalman(float data)
{ 
　　static float Xlast=0.01;//初值 Xlast&Xpre
　　static float P=0.1;//Plast&Pnow
　　const float Q=0.44;//自己感觉的方差
　　const float R=0.54;//测量器件的方差 
　　float Kg,PP;//
　　float Xnow;//经过卡尔曼滤波的值 Xnow

　　//1式A=1 无输入
　　PP=P+Q; //Ppre=Plast+Q （2）
　　Kg=PP/(PP+R); //更新Kg Ppre/Ppre+R（4）
　　Xnow=Xlast+Kg*(data-Xlast); // Xnow = Xpre+Kg*(Z(k)-H*Xpre)(3)
　　P=(1-Kg)*PP; //Pnow=(I-Kg)*Ppre(5) 由于Pnow不会再使用，所以更新Plast=Pnow
　　Xlast=Xnow; 

　　return Xnow;
}

二、高通滤波

三、融合滤波

3.1互补滤波

当有两种同一类型参数的时候，虽然不同它们的特性又刚好互有长短，为了各取所长，互补其短，所以叫互补滤波。以下为这种情况的举例：

在姿态角测量中 加速度计（偏移小、精度低）和 陀螺仪 （偏移大、发散、精度高）

在高度测量中 超声波传感器（偏移小、采样率低） 和 气压计（偏移大、采样率高）

在位置测量中 GPS（偏移小、采样率低） 和 惯性导航系统（偏移大、采样率高）

 

 以下这段程序以姿态角计算为例说明

提取Pitch轴说明： 陀螺仪读出角速度积分A，利用重力加速度读出加速度计可以计算出夹角B

制作工艺使得陀螺仪积分角度A随时间的推移发生零点偏移（角度产生严重的偏差），积分后产生越来越严重的累计误差。

另一方面，加速度计算夹角B没有积分过程不会产生如上问题，当物体运动时，物体本身的加速度没有被消除导致加速度计的值在物体加减速运动瞬间偏差。

故使用互补滤波让陀螺仪A为精确测量部件占一个大的比例，加速度B占一个小的比例作为修正

float FusionFilter_Complementary_1p(float AccAngle,float GyrRate,double PRYAngleLast,float dtC)
{
　　float tau=3;
　　float porprtion;
　　float RPYAngleNew;

　　porprtion=tau/(tau+dtC); 
　　RPYAngleNew=porprtion*(PRYAngleLast+GyrRate*dtC) + (1-porprtion)*(AccAngle);
　　return RPYAngleNew;
}

AccAngle:输入加速度计算夹角后的值，GyrRate：角速度，PRYAngleLast：上一次的姿态角（Pitch或Roll适用），dtC：两次调用的时间间隔用于积分

3.2卡尔曼滤波

与上面低通滤波中的卡尔曼滤波不同，这次的输入参数包括两种，以一种对另一种进行预测，以下是一段修改自互联网的程序，使用加速度计和陀螺仪来计算姿态角

void FusionFilter__Kalman_Filter_IMU(float Accel,float GyroRate,double *Angle ,double dt) 
{

static const float Q_angle=0.001; 
static const float Q_gyro=0.003;
static const float R_angle=0.5;
//    static const float dt=0.01;    
static const char C_0 = 1;
static float Q_bias, Angle_err;
static float PCt_0, PCt_1, E;
static float K_0, K_1, t_0, t_1;
static float Pdot[4] ={0,0,0,0};
static float PP[2][2] = { { 1, 0 },{ 0, 1 } };

*Angle +=(GyroRate - Q_bias) * dt; //先验估计

Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // pk-先验估计误差协方差的微分

Pdot[1]= -PP[1][1];
Pdot[2]= -PP[1][1];
Pdot[3]=Q_gyro;

PP[0][0] += Pdot[0] * dt; //pk-先验估计误差协方差微分的积分
PP[0][1] += Pdot[1] * dt; //=先验估计误差协方差
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;

Angle_err = Accel - *Angle;    //zk-先验估计

PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[1][0];

E = R_angle + C_0 * PCt_0;

K_0 = PCt_0 / E;
K_1 = PCt_1 / E;

t_0 = PCt_0;
t_1 = C_0 * PP[0][1];

PP[0][0] -= K_0 * t_0;    //后验估计误差协方差
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;

*Angle    += K_0 * Angle_err;    //后验算估计
Q_bias    += K_1 * Angle_err;    //后验估计
//    Gyro_y = GyroRate - Q_bias;    //输出值（后验估计）的微分 = 实际角速度
}