常用的濾波算法

大傻子的文淵閣 2019-07-17

展開全文

濾波是傳感器處理中的重要算法，經(jīng)常接觸底層常常用到，以下總結(jié)了一些濾波算法，供以后參考調(diào)用。

一、低通濾波

1.1RC濾波的數(shù)字低通濾波

　　指在截止頻率fc的時候，增益為-3db(Aup=0.707)的濾波器，也是模電書中出現(xiàn)的第一種硬件濾波器，以下是對應的軟件形式的1階RC濾波器的數(shù)字形式(本斷程序節(jié)選自匿名4軸)

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

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

 1 　　float LopPassFilter_RC_1st(float oldData, float newData, float a)
 2 　　{
 3 　　　　return oldData * (1 - a) + newData * a;
 4 　　}
 5 
 6 　　計算比例系數(shù)a:
 7 
 8 　　float LopPassFilter_RC_1st_Factor_Cal(float deltaT, float Fcut)
 9 　　{
10 　　　　return deltaT / (deltaT + 1 / (2 * M_PI * Fcut));
11 　　}

1.2均值濾波：

　　把一段時間內(nèi)的數(shù)據(jù)累加后求平均值，達到平滑的作用，適用性廣泛，元素越多濾波效果越好時延越高。

 1 　　uint16_t LowPassFilter_Average(uint16_t data[],uint16_t length)
 2 
 3 　　{
 4 
 5 　　　　uint32_t add=0;
 6 　　　　uint16_t result;
 7 　　　　int i;
 8 
 9 　　　　for(i=0;i<length;i++)
10 　　　　{
11 　　　　　　add += data[i];
12 　　　　}
13 　　　　result=add/length;
14 　　　　return result;
15 　　}
16 
17 　　//data[]放入一段時間里的數(shù)值，length：data數(shù)組的長度

1.3滑動濾波

　　在均值濾波的基礎上，加上比例系數(shù)，最新的數(shù)據(jù)具有更大的比例，增加時效性。

　　以下列舉了一個三個元素的滑動濾波

 1 　　double LowPassFilter_Silding(double dataNewest,double dataMiddle,double dataLast)
 2 　　{
 3 
 4 　　　　#define PROPORTIONNEW 0.55
 5 
 6 　　　　#define PROPORTIONMID 0.35
 7 
 8 　　　　#define PROPORTIONLAST (1- PROPORTIONNEW -PROPORTIONMID )
 9 
10 　　　　double result;
11 　　　　result = PROPORTIONNEW *dataNewest+ PROPORTIONMID *dataMiddle+PROPORTIONLAST *dataLast;
12 　　　　return result;
13 　　}

　　三個輸入?yún)?shù)為連續(xù)3個讀出的數(shù)據(jù)，其中 dataNewest 為最新數(shù)據(jù)，對應比例系數(shù)PROPORTIONNEW ，另外兩個同理。比例系數(shù)根據(jù)需要改變

1.4中值濾波

　　當傳感器采集的數(shù)據(jù)存在毛刺時，為了提取其中有效的數(shù)據(jù)，采用中值濾波的算法，只保存數(shù)據(jù)大小在中間的數(shù)值。

 1 　　uint16_t LowPassFilter_GetMiddleValue( uint16_t *pusBuffer, uint8_t ucLength )
 2 　　{
 3 　　　　uint8_t i;
 4 　　　　uint16_t usMiddle,usMax,usCount = 0,usPoint;
 5 
 6 　　　　while( usCount <= (ucLength/2) )
 7 　　　　{
 8 　　　　　　usPoint = 0;
 9 　　　　　　usMax = pusBuffer[0];
10 　　　　　　for( i=1;i<ucLength;i++ ) 
11 　　　　　　{ 
12 　　　　　　　　if( pusBuffer[i] > usMax ) 
13 　　　　　　　　{
14 　　　　　　　　　　usMax = pusBuffer[i];
15 　　　　　　　　　　usPoint = i;
16 　　　　　　　　} 
17 　　　　　　}
18 　　　　　　usMiddle = usMax;
19 　　　　　　pusBuffer[usPoint] = 0; 
20 　　　　　　usCount++; 
21 　　　　}
22 　　　　return usMiddle;
23 　　}

　　*pusBuffer:待改變的起始地址 ucLength:長度

　　特別注意：為了運行效率，這個方法中沒有對數(shù)組進行備份，會直接操作到源 *data并改變元素的值。

　　這個快速中值算法原理是只排序最大或者最小值的一半，一半中的最后一個即為中間值

1.5去除極值的XX濾波

　　把中值濾波的優(yōu)點結(jié)合給其他濾波，使其他濾波器也具有夠剔除少量毛刺的能力。

　　以下例子結(jié)合的是均值濾波，即去除極值的均值濾波

 1 　　int16_t LowPassFilter_RemoveExtremumAverage(int16_t data[],int16_t length)
 2 　　{
 3 　　　　#define EXTERMUM_NUMBER 1 //number of extermum
 4 　　　　int32_t sum=0;
 5 　　　　int16_t i;
 6 
 7 　　　　if(length<=2*EXTERMUM_NUMBER)
 8 　　　　{
 9 　　　　　　return 0;//length shorter than 2*EXTERMUM_NUMBER, return err
10 　　　　}
11 
12 　　　　//sort
13 　　　　BubbleSort_int16(data,length);
14 　　　　
15 
16 　　　　//average filter
17 　　　　for(i=EXTERMUM_NUMBER;i<length-EXTERMUM_NUMBER;i++)
18 　　　　{
19 　　　　　　sum+=data[i];
20 　　　　}
21 
22 　　　　return sum/(length-2*EXTERMUM_NUMBER);
23 　　}

　　輸入長度為length的數(shù)組data[]。EXTERMUM_NUMBER定義去除極值的個數(shù)（1表示去掉1個最大值和1個最小值）。先用排序法（本例使用下面給出的冒泡排序）對data數(shù)組進行排序，去除1個最大值和1個最小值后剩余求平均值。

　　這段代碼可能出現(xiàn)兩個錯誤

　　1、length比2倍的EXTERMUM_NUMBER短，本例用return 0一筆帶過，實際需要在程序return 0處作處理

　　2、如果輸入length過長會使32位的sum在累加過程溢出，如果length很長建議改變均值濾波的順序在累加之前線進行除法

　　下面給出子程序冒泡排序

 1 　　void BubbleSort_int16(int16_t *a,int len)
 2 　　{
 3 　　　　int i;
 4 　　　　int j;
 5 　　　　int mid;
 6 　　　　for(i=0;i<len;i++)
 7 　　　　{
 8 　　　　　　for(j=0;j<len-i-1;j++)
 9 　　　　　　{
10 　　　　　　　　if(*(a+j)>*(a+j+1))
11 　　　　　　　　{
12 　　　　　　　　　　mid=*(a+j);
13 　　　　　　　　　　*(a+j)=*(a+j+1);
14 　　　　　　　　　　*(a+j+1)=mid;
15 　　　　　　　　}
16 　　　　　　}
17 　　　　}
18 　　}

1.6巴特沃斯低通濾波

　　借助MATLAB使用數(shù)字濾波的方法來模擬巴特沃斯濾波器：

　　IIR濾波的基本結(jié)構(gòu) 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為階數(shù)，x(n)為本次輸入，x(n-1)為上一次輸入，如此類推x(n-i)為i次前的輸入。y(n)為輸出，y(n-1)為上一次的輸出。y(n-i)為i次前的輸出。 a和b分別為比例。

　　系數(shù)a和b改變達到模擬不同采樣頻率、不同截止頻率的濾波效果。

　　以下使用matlab計算一個1000HZ采樣頻率，100HZ截止頻率，2階的巴特沃斯濾波器的IIR系數(shù)：

　　>>fdatool

1.7切比雪夫低通濾波

1.8卡爾曼濾波

卡爾曼濾波是一種預測型濾波，這里把卡爾曼濾波歸納到低通濾波中，是因為在適用過程中卡爾曼增益Kg會慢慢收斂，變成跟低通濾波一樣的效果

 1 float LowPassFilter_kalman(float data)
 2 { 
 3 　　static float Xlast=0.01;//初值 Xlast&Xpre
 4 　　static float P=0.1;//Plast&Pnow
 5 　　const float Q=0.44;//自己感覺的方差
 6 　　const float R=0.54;//測量器件的方差 
 7 　　float Kg,PP;//
 8 　　float Xnow;//經(jīng)過卡爾曼濾波的值 Xnow
 9 
10 　　//1式A=1 無輸入
11 　　PP=P+Q; //Ppre=Plast+Q （2）
12 　　Kg=PP/(PP+R); //更新Kg Ppre/Ppre+R（4）
13 　　Xnow=Xlast+Kg*(data-Xlast); // Xnow = Xpre+Kg*(Z(k)-H*Xpre)(3)
14 　　P=(1-Kg)*PP; //Pnow=(I-Kg)*Ppre(5) 由于Pnow不會再使用，所以更新Plast=Pnow
15 　　Xlast=Xnow; 
16 
17 　　return Xnow;
18 }

二、高通濾波

三、融合濾波

3.1互補濾波

當有兩種同一類型參數(shù)的時候，雖然不同它們的特性又剛好互有長短，為了各取所長，互補其短，所以叫互補濾波。以下為這種情況的舉例：

在姿態(tài)角測量中加速度計（偏移小、精度低）和陀螺儀（偏移大、發(fā)散、精度高）

在高度測量中超聲波傳感器（偏移小、采樣率低）和氣壓計（偏移大、采樣率高）

在位置測量中 GPS（偏移小、采樣率低）和慣性導航系統(tǒng)（偏移大、采樣率高）

以下這段程序以姿態(tài)角計算為例說明

提取Pitch軸說明：陀螺儀讀出角速度積分A，利用重力加速度讀出加速度計可以計算出夾角B

制作工藝使得陀螺儀積分角度A隨時間的推移發(fā)生零點偏移（角度產(chǎn)生嚴重的偏差），積分后產(chǎn)生越來越嚴重的累計誤差。

另一方面，加速度計算夾角B沒有積分過程不會產(chǎn)生如上問題，當物體運動時，物體本身的加速度沒有被消除導致加速度計的值在物體加減速運動瞬間偏差。

故使用互補濾波讓陀螺儀A為精確測量部件占一個大的比例，加速度B占一個小的比例作為修正

 1 float FusionFilter_Complementary_1p(float AccAngle,float GyrRate,double PRYAngleLast,float dtC)
 2 {
 3 　　float tau=3;
 4 　　float porprtion;
 5 　　float RPYAngleNew;
 6 
 7 　　porprtion=tau/(tau+dtC); 
 8 　　RPYAngleNew=porprtion*(PRYAngleLast+GyrRate*dtC) + (1-porprtion)*(AccAngle);
 9 　　return RPYAngleNew;
10 }

AccAngle:輸入加速度計算夾角后的值，GyrRate：角速度，PRYAngleLast：上一次的姿態(tài)角（Pitch或Roll適用），dtC：兩次調(diào)用的時間間隔用于積分

3.2卡爾曼濾波

與上面低通濾波中的卡爾曼濾波不同，這次的輸入?yún)?shù)包括兩種，以一種對另一種進行預測，以下是一段修改自互聯(lián)網(wǎng)的程序，使用加速度計和陀螺儀來計算姿態(tài)角

 1 void FusionFilter__Kalman_Filter_IMU(float Accel,float GyroRate,double *Angle ,double dt) 
 2 {
 3 
 4 static const float Q_angle=0.001; 
 5 static const float Q_gyro=0.003;
 6 static const float R_angle=0.5;
 7 //    static const float dt=0.01;    
 8 static const char C_0 = 1;
 9 static float Q_bias, Angle_err;
10 static float PCt_0, PCt_1, E;
11 static float K_0, K_1, t_0, t_1;
12 static float Pdot[4] ={0,0,0,0};
13 static float PP[2][2] = { { 1, 0 },{ 0, 1 } };
14 
15 *Angle +=(GyroRate - Q_bias) * dt; //先驗估計
16 
17 Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // pk-先驗估計誤差協(xié)方差的微分
18 
19 Pdot[1]= -PP[1][1];
20 Pdot[2]= -PP[1][1];
21 Pdot[3]=Q_gyro;
22 
23 PP[0][0] += Pdot[0] * dt; //pk-先驗估計誤差協(xié)方差微分的積分
24 PP[0][1] += Pdot[1] * dt; //=先驗估計誤差協(xié)方差
25 PP[1][0] += Pdot[2] * dt;
26 PP[1][1] += Pdot[3] * dt;
27 
28 Angle_err = Accel - *Angle;    //zk-先驗估計
29 
30 PCt_0 = C_0 * PP[0][0];
31 PCt_1 = C_0 * PP[1][0];
32 
33 E = R_angle + C_0 * PCt_0;
34 
35 K_0 = PCt_0 / E;
36 K_1 = PCt_1 / E;
37 
38 t_0 = PCt_0;
39 t_1 = C_0 * PP[0][1];
40 
41 PP[0][0] -= K_0 * t_0;    //后驗估計誤差協(xié)方差
42 PP[0][1] -= K_0 * t_1;
43 PP[1][0] -= K_1 * t_0;
44 PP[1][1] -= K_1 * t_1;
45 
46 *Angle    += K_0 * Angle_err;    //后驗算估計
47 Q_bias    += K_1 * Angle_err;    //后驗估計
48 //    Gyro_y = GyroRate - Q_bias;    //輸出值（后驗估計）的微分 = 實際角速度
49 }