濾波是傳感器處理中的重要算法，經常接觸底層常常用到，以下總結了一些濾波算法，供以后參考調用。
下文分為三部分
1、低通濾波
2、高通濾波
3、融合濾波
一、低通濾波
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<length;i++)
{
add += data[i];
}
result=add/length;
return result;
}
data[]放入一段時間里的數值，length：data數組的長度
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中值濾波
當傳感器采集的數據存在毛刺時，為了提取其中有效的數據，采用中值濾波的算法，只保存數據大小在中間的數值，這里特別鳴謝在@l劉土生 朋友幫助下，編寫了一種簡單有效快速找中值的算法。
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<ucLength;i++ )
{
if( pusBuffer[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-EXTERMUM_NUMBER;i++)
{
sum+=data[i];
}
return sum/(length-2*EXTERMUM_NUMBER);
}
輸入長度為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<len;i++)
{
for(j=0;j<len-i-1;j++)
{
if(*(a+j)>*(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; //輸出值（后驗估計）的微分 = 實際角速度
}