|
濾波是傳感器處理中的重要算法，經(jīng)常接觸底層常常用到，以下總結(jié)了一些濾波算法，供以后參考調(diào)用。 一、低通濾波1.1RC濾波的數(shù)字低通濾波指在截止頻率fc的時(shí)候，增益為-3db(Aup=0.707)的濾波器，也是模電書中出現(xiàn)的第一種硬件濾波器，以下是對(duì)應(yīng)的軟件形式的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 計(jì)算比例系數(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均值濾波：把一段時(shí)間內(nèi)的數(shù)據(jù)累加后求平均值，達(dá)到平滑的作用，適用性廣泛，元素越多濾波效果越好時(shí)延越高。 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í)間里的數(shù)值，length：data數(shù)組的長(zhǎng)度
1.3滑動(dòng)濾波在均值濾波的基礎(chǔ)上，加上比例系數(shù)，最新的數(shù)據(jù)具有更大的比例，增加時(shí)效性。 以下列舉了一個(gè)三個(gè)元素的滑動(dòng)濾波 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 }
三個(gè)輸入?yún)?shù)為連續(xù)3個(gè)讀出的數(shù)據(jù)，其中 dataNewest 為最新數(shù)據(jù)，對(duì)應(yīng)比例系數(shù)PROPORTIONNEW ，另外兩個(gè)同理。比例系數(shù)根據(jù)需要改變 1.4中值濾波當(dāng)傳感器采集的數(shù)據(jù)存在毛刺時(shí)，為了提取其中有效的數(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:長(zhǎng)度 特別注意：為了運(yùn)行效率，這個(gè)方法中沒有對(duì)數(shù)組進(jìn)行備份，會(huì)直接操作到源 *data并改變?cè)氐闹怠?/p> 這個(gè)快速中值算法原理是只排序最大或者最小值的一半，一半中的最后一個(gè)即為中間值 1.5去除極值的XX濾波把中值濾波的優(yōu)點(diǎn)結(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 } 輸入長(zhǎng)度為length的數(shù)組data[]。EXTERMUM_NUMBER定義去除極值的個(gè)數(shù)（1表示去掉1個(gè)最大值和1個(gè)最小值）。先用排序法（本例使用下面給出的冒泡排序）對(duì)data數(shù)組進(jìn)行排序，去除1個(gè)最大值和1個(gè)最小值后剩余求平均值。 這段代碼可能出現(xiàn)兩個(gè)錯(cuò)誤 1、length比2倍的EXTERMUM_NUMBER短，本例用return 0一筆帶過，實(shí)際需要在程序return 0處作處理 2、如果輸入length過長(zhǎng)會(huì)使32位的sum在累加過程溢出，如果length很長(zhǎng)建議改變均值濾波的順序在累加之前線進(jìn)行除法 下面給出子程序冒泡排序  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); 這里簡(jiǎn)單把IIR濾波理解為滑動(dòng)濾波的進(jìn)階形式，其中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改變達(dá)到模擬不同采樣頻率、不同截止頻率的濾波效果。 以下使用matlab計(jì)算一個(gè)1000HZ采樣頻率，100HZ截止頻率，2階的巴特沃斯濾波器的IIR系數(shù)： >>fdatool
1.7切比雪夫低通濾波1.8卡爾曼濾波卡爾曼濾波是一種預(yù)測(cè)型濾波，這里把卡爾曼濾波歸納到低通濾波中，是因?yàn)樵谶m用過程中卡爾曼增益Kg會(huì)慢慢收斂，變成跟低通濾波一樣的效果 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;//測(cè)量器件的方差 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不會(huì)再使用，所以更新Plast=Pnow 15 Xlast=Xnow; 16 17 return Xnow; 18 } 二、高通濾波三、融合濾波3.1互補(bǔ)濾波當(dāng)有兩種同一類型參數(shù)的時(shí)候，雖然不同它們的特性又剛好互有長(zhǎng)短，為了各取所長(zhǎng)，互補(bǔ)其短，所以叫互補(bǔ)濾波。以下為這種情況的舉例： 在姿態(tài)角測(cè)量中 加速度計(jì)（偏移小、精度低）和 陀螺儀 （偏移大、發(fā)散、精度高） 在高度測(cè)量中 超聲波傳感器（偏移小、采樣率低） 和 氣壓計(jì)（偏移大、采樣率高） 在位置測(cè)量中 GPS（偏移小、采樣率低） 和 慣性導(dǎo)航系統(tǒng)（偏移大、采樣率高）
以下這段程序以姿態(tài)角計(jì)算為例說明 提取Pitch軸說明： 陀螺儀讀出角速度積分A，利用重力加速度讀出加速度計(jì)可以計(jì)算出夾角B 制作工藝使得陀螺儀積分角度A隨時(shí)間的推移發(fā)生零點(diǎn)偏移（角度產(chǎn)生嚴(yán)重的偏差），積分后產(chǎn)生越來越嚴(yán)重的累計(jì)誤差。 另一方面，加速度計(jì)算夾角B沒有積分過程不會(huì)產(chǎn)生如上問題，當(dāng)物體運(yùn)動(dòng)時(shí)，物體本身的加速度沒有被消除導(dǎo)致加速度計(jì)的值在物體加減速運(yùn)動(dòng)瞬間偏差。 故使用互補(bǔ)濾波讓陀螺儀A為精確測(cè)量部件占一個(gè)大的比例，加速度B占一個(gè)小的比例作為修正 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:輸入加速度計(jì)算夾角后的值，GyrRate：角速度，PRYAngleLast：上一次的姿態(tài)角（Pitch或Roll適用），dtC：兩次調(diào)用的時(shí)間間隔用于積分 3.2卡爾曼濾波與上面低通濾波中的卡爾曼濾波不同，這次的輸入?yún)?shù)包括兩種，以一種對(duì)另一種進(jìn)行預(yù)測(cè)，以下是一段修改自互聯(lián)網(wǎng)的程序，使用加速度計(jì)和陀螺儀來計(jì)算姿態(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; //先驗(yàn)估計(jì) 16 17 Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // pk-先驗(yàn)估計(jì)誤差協(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-先驗(yàn)估計(jì)誤差協(xié)方差微分的積分 24 PP[0][1] += Pdot[1] * dt; //=先驗(yàn)估計(jì)誤差協(xié)方差 25 PP[1][0] += Pdot[2] * dt; 26 PP[1][1] += Pdot[3] * dt; 27 28 Angle_err = Accel - *Angle; //zk-先驗(yàn)估計(jì) 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; //后驗(yàn)估計(jì)誤差協(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; //后驗(yàn)算估計(jì) 47 Q_bias += K_1 * Angle_err; //后驗(yàn)估計(jì) 48 // Gyro_y = GyroRate - Q_bias; //輸出值（后驗(yàn)估計(jì)）的微分 = 實(shí)際角速度 49 } 轉(zhuǎn)載：https://www.cnblogs.com/HongYi-Liang/p/7002718.html
|
|
|
