常用的濾波演算法
濾波是感測器處理中的重要演算法，經常接觸底層常常用到，以下總結了一些濾波演算法，供以後參考呼叫。
一、低通濾波
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)
1 float LopPassFilter_RC_1st(float oldData, floatnewData, float a) 2 { 3 return oldData * (1 - a) + newData * a; 4 } 5 6 計算比例係數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均值濾波：
把一段時間內的資料累加後求平均值，達到平滑的作用，適用性廣泛，元素越多濾波效果越好時延越高。
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[]放入一段時間裡的數值，length：data陣列的長度
1.3滑動濾波
在均值濾波的基礎上，加上比例係數，最新的資料具有更大的比例，增加時效性。
以下列舉了一個三個元素的滑動濾波
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 }
三個輸入引數為連續3個讀出的資料，其中 dataNewest 為最新資料，對應比例係數PROPORTIONNEW ，另外兩個同理。比例係數根據需要改變
1.4中值濾波
當感測器採集的資料存在毛刺時，為了提取其中有效的資料，採用中值濾波的演算法，只儲存資料大小在中間的數值，這裡特別鳴謝在@l劉土生 朋友幫助下，編寫了一種簡單有效快速找中值的演算法。
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:長度
特別注意：為了執行效率，這個方法中沒有對陣列進行備份，會直接操作到源 *data並改變元素的值。
這個快速中值演算法原理是隻排序最大或者最小值的一半，一半中的最後一個即為中間值
1.5去除極值的XX濾波
把中值濾波的優點結合給其他濾波，使其他濾波器也具有夠剔除少量毛刺的能力。
以下例子結合的是均值濾波，即去除極值的均值濾波
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的陣列data[]。EXTERMUM_NUMBER定義去除極值的個數（1表示去掉1個最大值和1個最小值）。先用排序法（本例使用下面給出的氣泡排序）對data陣列進行排序，去除1個最大值和1個最小值後剩餘求平均值。
這段程式碼可能出現兩個錯誤
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使用數字濾波的方法來模擬巴特沃斯濾波器：
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會慢慢收斂，變成跟低通濾波一樣的效果
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;//經過卡爾曼濾波的值 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互補濾波
當有兩種同一型別引數的時候，雖然不同它們的特性又剛好互有長短，為了各取所長，互補其短，所以叫互補濾波。以下為這種情況的舉例：
在姿態角測量中 加速度計（偏移小、精度低）和 陀螺儀 （偏移大、發散、精度高）
在高度測量中 超聲波感測器（偏移小、取樣率低） 和 氣壓計（偏移大、取樣率高）
在位置測量中 GPS（偏移小、取樣率低） 和 慣性導航系統（偏移大、取樣率高）
以下這段程式以姿態角計算為例說明
提取Pitch軸說明： 陀螺儀讀出角速度積分A，利用重力加速度讀出加速度計可以計算出夾角B
製作工藝使得陀螺儀積分角度A隨時間的推移發生零點偏移（角度產生嚴重的偏差），積分後產生越來越嚴重的累計誤差。
另一方面，加速度計算夾角B沒有積分過程不會產生如上問題，當物體運動時，物體本身的加速度沒有被消除導致加速度計的值在物體加減速運動瞬間偏差。
故使用互補濾波讓陀螺儀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：上一次的姿態角（Pitch或Roll適用），dtC：兩次呼叫的時間間隔用於積分
3.2卡爾曼濾波
與上面低通濾波中的卡爾曼濾波不同，這次的輸入引數包括兩種，以一種對另一種進行預測，以下是一段修改自網際網路的程式，使用加速度計和陀螺儀來計算姿態角
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-先驗估計誤差協方差的微分 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-先驗估計誤差協方差微分的積分 24 PP[0][1] += Pdot[1] * dt; //=先驗估計誤差協方差 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; //後驗估計誤差協方差 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 }
轉載：https://www.cnblogs.com/HongYi-Liang/p/7002718.html