濾波是傳感器處理中的重要算法，經(jīng)常接觸底層常常用到，以下總結(jié)了一些濾波算法，供以后參考調(diào)用。
指在截止頻率fc的時候，增益為-3db(Aup=0.707)的濾波器，也是模電書中出現(xiàn)的第一種硬件濾波器，以下是對應(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 計算比例系數(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 }
把一段時間內(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ù)組的長度
在均值濾波的基礎(chǔ)上，加上比例系數(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ù)，對應(yīng)比例系數(shù)PROPORTIONNEW ，另外兩個同理。比例系數(shù)根據(jù)需要改變
當(dāng)傳感器采集的數(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并改變元素的值。
這個快速中值算法原理是只排序最大或者最小值的一半，一半中的最后一個即為中間值
把中值濾波的優(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 err10 }11 12 //sort13 BubbleSort_int16(data,length);14 15 16 //average filter17 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 }
借助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
卡爾曼濾波是一種預(yù)測型濾波，這里把卡爾曼濾波歸納到低通濾波中，是因為在適用過程中卡爾曼增益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=Pnow15 Xlast=Xnow; 16 17 return Xnow;18 }
當(dāng)有兩種同一類型參數(shù)的時候，雖然不同它們的特性又剛好互有長短，為了各取所長，互補其短，所以叫互補濾波。以下為這種情況的舉例：
在姿態(tài)角測量中 加速度計（偏移小、精度低）和 陀螺儀 （偏移大、發(fā)散、精度高）
在高度測量中 超聲波傳感器（偏移小、采樣率低） 和 氣壓計（偏移大、采樣率高）
在位置測量中 GPS（偏移小、采樣率低） 和 慣性導(dǎo)航系統(tǒng)（偏移大、采樣率高）
以下這段程序以姿態(tài)角計算為例說明
提取Pitch軸說明： 陀螺儀讀出角速度積分A，利用重力加速度讀出加速度計可以計算出夾角B
制作工藝使得陀螺儀積分角度A隨時間的推移發(fā)生零點偏移（角度產(chǎn)生嚴重的偏差），積分后產(chǎn)生越來越嚴重的累計誤差。
另一方面，加速度計算夾角B沒有積分過程不會產(chǎn)生如上問題，當(dāng)物體運動時，物體本身的加速度沒有被消除導(dǎo)致加速度計的值在物體加減速運動瞬間偏差。
故使用互補濾波讓陀螺儀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)用的時間間隔用于積分
與上面低通濾波中的卡爾曼濾波不同，這次的輸入?yún)?shù)包括兩種，以一種對另一種進行預(yù)測，以下是一段修改自互聯(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 }
轉(zhuǎn)載：https://www.cnblogs.com/HongYi-Liang/p/7002718.html
