C/C++ 11个过滤算法代码总结
转载自极客工坊：http://www.405452.com/thread-7694-1-1.html。感谢原作者沉海宇的总结工作。
原作者在Arduino平台上运行这些代码，使用的语言是C/C++。
1。极限滤波法（程序判断滤波法）
2. 中值滤波法
3. 算术平均滤波法
4. 递归平均滤波法（移动平均滤波法）
5. 中值平均滤波法（抗脉冲干扰平均滤波法）
6. 极限平均滤波法
7. 一阶滞后滤波法
8. 加权递归平均滤波法
9. 防跳动滤波法
10. 限制防跳滤波方法
11. 新增卡尔曼滤波器（非扩展卡尔曼）
程序默认过滤int类型数据。如果需要过滤其他类型，只需要将程序中的所有int替换为long、float或double即可。
1。限制过滤法（程序判断过滤法）
/*
A.名称：限制过滤法（程序判断过滤法）
B、方法：根据经验判断，确定两次采样所允许的最大偏差值（设为A）。每次检测到新值时，判断：如果本次值与上次值相差<=A，则本次值有效，如果本次值与上次值之差>A，则本次值无效，则丢弃本次值，用上次值替换。
C、优点：可以有效克服偶然因素造成的脉冲干扰。
D.缺点：无法抑制周期性干扰。光滑度差。
E、主办单位：深海宇 2013-11-01
*/int Filter_Value;
int Value;void setup() {Serial.begin(9600); // 初始化串行通信 randomSeed(analogRead(0)); // 生成随机种子 Value = 300;
}void 循环() {Filter_Value = Filter(); // 获取过滤器输出值 Value = Filter_Value; // 最新有效样本的值，该变量为全局变量 Serial.println(Filter_Value); // 串口输出延时(50);}//用于随机生成300左右的当前值
int Get_AD() {返回随机数(295, 305);
}//限制过滤方法（程序判断过滤方法）
#定义FILTER_A 1
int Filter() {int NewValue;NewValue = Get_AD();if(((NewValue - Value) > FILTER_A) || ((Value - NewValue) > FILTER_A))return Value;elsereturn NewValue;
}2.中值滤波法
/*
A.名称：中值滤波法
B.方法：连续采样N次（N为奇数），将N次采样值按照大小排列，取中间值作为本次有效值。
C、优点：可以有效克服偶然因素造成的波动干扰；对温度、液位变化缓慢的测量参数有良好的滤波效果。
D、缺点：不适合流量、速度等快速变化的参数。
E、主办单位：深海宇 2013-11-01
*/int Filter_Value;void setup() {Serial.begin(9600); // 初始化串行通信 randomSeed(analogRead(0)); // 生成随机种子
}void 循环() {Filter_Value = Filter(); // 获取过滤器输出值 Serial.println(Filter_Value); // 串口输出延时(50);
}//用于随机生成300左右的当前值
int Get_AD() {返回随机数(295, 305);
}//中值滤波方法
#定义FILTER_N 101int Filter() {int filter_buf[FILTER_N];int i, j;int filter_temp;for(i = 0; i < FILTER_N; i++) {filter_buf[i] = Get_AD();delay(1);}// 采样值从小到大排列（冒泡法）for(j = 0; j < FILTER_N - 1; j++) {for(i = 0; i < FILTER_N - 1 - j; i++) {if(filter_buf[i] > filter_buf[i + 1]) {filter_temp = filter_buf[i];filter_buf[i] = filter_buf [i + 1];filter_buf[i + 1] = filter_temp;}}}返回filter_buf[(FILTER_N - 1) / 2];
}3.算术平均滤波法
/*
A.名称：算术平均滤波法
B、方法：连续取N个采样值，进行算术平均计算：N值越大：信号平滑度越高，但灵敏度越低； N值越小：信号平滑度越低，但灵敏度越高； N值选择：一般流量，N=12；压力：N=4。
C、优点：适合滤除一般有随机干扰的信号；该信号的特点是平均值，信号在一定数值范围附近上下波动。
D、缺点：不适合要求测量速度慢或数据计算速度快的实时控制；这是对内存的浪费。
E、主办单位：深海宇 2013-11-01
*/int Filter_Value;void setup() {Serial.begin(9600); // 初始化串行通信 randomSeed(analogRead(0)); // 生成随机种子
}void 循环() {Filter_Value = Filter(); // 获取过滤器输出值 Serial.println(Filter_Value); // 串口输出延时(50);
}//用于随机生成300左右的当前值
int Get_AD() {返回随机数(295, 305);
}//算术平均滤波方法
#定义FILTER_N 12
int Filter() {int i;int filter_sum = 0;for(i = 0; i < FILTER_N; i++) {filter_sum += Get_AD();delay(1);}return (int)(filter_sum / FILTER_N);}4.递归平均滤波法（移动平均滤波法）
/*
A.名称：递归平均滤波法（也叫滑动平均滤波法）
B.方法：将N个连续的样本值视为一个队列。队列长度固定为N，每次采样到一个新的数据，放入队列尾部，丢弃队列头部的原始数据（先进先出原则），执行对队列中的N个数据进行算术平均运算，得到新的过滤结果。 N值的选择：流量，N=12；压力，N=4；液位，N=4-12；温度，N=1-4。
C、优点：对周期性干扰抑制效果好，平滑度高；适用于高频振荡系统。
D.缺点：灵敏度低，对偶发脉冲干扰的抑制能力差；脉冲干扰造成的采样值偏差难以消除；不适合脉冲干扰严重的场合；浪费内存。
E、主办单位：深海宇 2013-11-01
*/int Filter_Value;void setup() {Serial.begin(9600); // 初始化串行通信 randomSeed(analogRead(0)); // 生成随机种子
}void 循环() {Filter_Value = Filter(); // 获取过滤器输出值 Serial.println(Filter_Value); // 串口输出延时(50);
}//用于随机生成300左右的当前值
int Get_AD() {返回随机数(295, 305);
}//递归平均滤波法（也叫移动平均滤波法）
#定义FILTER_N 12
int filter_buf[FILTER_N + 1];
int Filter() {int i;int filter_sum = 0;filter_buf[FILTER_N] = Get_AD();for(i = 0; i < FILTER_N; i++) {filter_buf[i] = filter_buf[i + 1]; // 所有数据左移，低位仍被丢弃 filter_sum += filter_buf[i];}return (int)(filter_sum / FILTER_N);}5.中值平均滤波法（抗脉冲干扰平均滤波法）
/*
A、名称：中值平均滤波法（抗脉冲干扰平均滤波法）
B.方法：取一组队列去掉最大值和最小值再取平均值，相当于“中值滤波法”+“算术均值滤波法”。连续采样N个数据，去掉一个最大值和一个最小值，然后计算N-2个数据的算术平均值。 N值的选择：3-14。
C、优点：它综合了“中值滤波法”+“算术均值滤波法”两种滤波方法的优点。对于偶发的脉冲干扰，可以消除其造成的采样值偏差。对周期性干扰有良好的抑制作用。平滑度高，适用于高频振荡系统。
D、缺点：计算速度慢，与算术平均滤波法相同。相当浪费内存。
E、主办单位：深海宇 2013-11-01
*/int Filter_Value;void setup() {Serial.begin(9600); // 初始化串行通信 randomSeed(analogRead(0)); // 生成随机种子
}void 循环() {Filter_Value = Filter(); // 获取过滤器输出值 Serial.println(Filter_Value); // 串口输出延时(50);
}//用于随机生成300左右的当前值
int Get_AD() {返回随机数(295, 305);
} // 中值平均滤波法（又称抗脉冲干扰平均滤波法）（算法1）
#定义FILTER_N 100int Filter() {int i, j;int filter_temp, filter_sum = 0;int filter_buf[FILTER_N];for(i = 0; i < FILTER_N; i++) {filter_buf[i] = Get_AD();delay(1);}// 采样值从小到大排列（冒泡法）for(j = 0; j < FILTER_N - 1; j++) {for(i = 0; i < FILTER_N - 1 - j; i++) {if(filter_buf[i] > filter_buf[i + 1]) {filter_temp = filter_buf[i];filter_buf[ i] = filter_buf[i + 1];filter_buf[i + 1] = filter_temp;}}}//去掉最大最小极值然后平均 for(i = 1; i < FILTER_N - 1; i++) filter_sum += filter_buf [i];返回filter_sum / (FILTER_N - 2);
} // 中值平均滤波法（抗脉冲干扰平均滤波法）（算法2）
/*
#定义FILTER_N 100
int Filter() {int i;int filter_sum = 0;int filter_max, filter_min;int filter_buf[FILTER_N];for(i = 0; i < FILTER_N; i++) {filter_buf[i] = Get_AD();delay(1);}filter_max = filter_buf[0];filter_min = filter_buf[0];filter_sum = filter_buf[0];for(i = FILTER_N - 1; i > 0; i--) {if(filter_buf[i] > filter_max) filter_max=filter_buf[i];else if(filter_buf[i] < filter_min)filter_min=filter_buf[i];filter_sum = filter_sum + filter_buf[i];filter_buf[i] = filter_buf[i - 1];}i = FILTER_N - 2;filter_sum = filter_sum - filter_max - filter_min + i / 2; // +i/2的目的是舍入filter_sum = filter_sum / i;return filter_sum;
}*/
6. 极限平均滤波法
/*
A.名称：极限平均滤波法
B.方法：相当于“极限滤波法”+“递归平均滤波法”；每个新采样的数据首先进行限幅处理，然后送入队列进行递归平均滤波处理。C、优点：结合了两种过滤方式的优点；对于偶发的脉冲干扰，可以消除脉冲干扰引起的采样值偏差。
D.缺点：浪费RAM。
E、主办单位：深海宇 2013-11-01
*/#定义FILTER_N 12
int Filter_Value；
int filter_buf[FILTER_N];void setup() {Serial.begin(9600); // 初始化串行通信 randomSeed(analogRead(0)); // 生成随机种子 filter_buf[FILTER_N - 2] = 300;
}void 循环() {Filter_Value = Filter(); // 获取过滤器输出值 Serial.println(Filter_Value); // 串口输出延时(50);
}//用于随机生成300左右的当前值
int Get_AD() {返回随机数(295, 305);
}//有限平均滤波方法
#定义FILTER_A 1
int Filter() {int i;int filter_sum = 0;filter_buf[FILTER_N - 1] = Get_AD();if(((filter_buf[FILTER_N - 1] - filter_buf[FILTER_N - 2]) > FILTER_A) || ((filter_buf [FILTER_N - 2] - filter_buf[FILTER_N - 1]) > FILTER_A))filter_buf[FILTER_N - 1] = filter_buf[FILTER_N - 2];for(i = 0; i < FILTER_N - 1; i++) {filter_buf[i] = filter_buf[i + 1];filter_sum += filter_buf[i];}return (int)filter_sum / (FILTER_N - 1);
}7。一阶滞后滤波法
/*
A.名称：一阶滞后滤波法B、方法：取a=0-1，本次滤波结果=(1-a)*本次采样值+a*上次滤波结果。
C、优点：对周期性干扰有良好的抑制效果；适用于波动频率较高的场合。
D.缺点：相位滞后，灵敏度低；滞后程度取决于a的值；滤波频率高于采样频率1/2的干扰信号无法消除。
E、主办单位：深海宇 2013-11-01
*/int Filter_Value;
int Value;void setup() {Serial.begin(9600); // 初始化串行通信 randomSeed(analogRead(0)); // 生成随机种子 Value = 300;
}void 循环() {Filter_Value = Filter(); // 获取过滤器输出值 Serial.println(Filter_Value); // 串口输出延时(50);
}//用于随机生成300左右的当前值
int Get_AD() {返回随机数(295, 305);
}//一阶滞后滤波方法
#定义FILTER_A 0.01
int Filter() {int NewValue;NewValue = Get_AD();Value = (int)((float)NewValue * FILTER_A + (1.0 - FILTER_A) * (float)Value);返回值；
}8。加权递归平均滤波法
/*
A.名称：加权递归平均滤波法
B.方法：是对递归平均滤波方法的改进，即对不同时刻的数据赋予不同的权重；通常，越接近当前时间的数据，权重越大。赋予新样本值的权重系数越大，灵敏度越高，但信号的平滑度越低。
C、优点：适用于纯延迟时间常数较大的对象和采样周期较短的系统。
D、缺点：对于纯延迟时间常数小、采样周期长、变化慢的信号；不能快速响应当前对系统的干扰严重程度，滤波效果较差。
E、主办单位：深海宇 2013-11-01*/int Filter_Value;void setup() {Serial.begin(9600); // 初始化串行通信 randomSeed(analogRead(0)); // 生成随机种子
}void 循环() {Filter_Value = Filter(); // 获取过滤器输出值 Serial.println(Filter_Value); // 串口输出延时(50);
}//用于随机生成300左右的当前值
int Get_AD() {返回随机数(295, 305);
}//加权递归平均滤波方法
#定义FILTER_N 12
int coe[FILTER_N] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12}; // 加权系数表
int sum_coe = 1 + 2 + 3 + 4 + 5 + 6 + 7 + 8 + 9 + 10 + 11 + 12; // 加权系数之和
int filter_buf[FILTER_N + 1];
int Filter() {int i;int filter_sum = 0;filter_buf[FILTER_N] = Get_AD();for(i = 0; i < FILTER_N; i++) {filter_buf[i] = filter_buf[i + 1]; // 所有数据左移，低位仍被丢弃 filter_sum += filter_buf[i] * coe[i];}filter_sum /= sum_coe;return filter_sum;
}
/*
A.名称：防跳过滤法
B、方法：设置一个过滤计数器，将每个采样值与当前有效值进行比较：如果采样值=当前有效值，则计数器清零；若采样值<>为当前有效值，则计数器+1，并判断计数器是否>=上限N（溢出）；如果计数器溢出，则用该值替换当前有效值并清除计数器。
C、优点：对于缓慢变化的测量参数有较好的滤波效果；它可以避免控制器在临界值附近重复开/关跳跃或显示器上的数字抖动。D、缺点：不适合快速变化的参数；如果计数器溢出时采样的值恰好是干扰值，则将该干扰值作为有效值导入到系统中。
E、主办单位：深海宇 2013-11-01
*/int Filter_Value;
int Value;void setup() {Serial.begin(9600); // 初始化串行通信 randomSeed(analogRead(0)); // 生成随机种子 Value = 300;
}void 循环() {Filter_Value = Filter(); // 获取过滤器输出值 Serial.println(Filter_Value); // 串口输出延时(50);
}//用于随机生成300左右的当前值
int Get_AD() {返回随机数(295, 305);
}//Debounce过滤方法
#定义FILTER_N 12
整数 i = 0;
int Filter() {int new_value;new_value = Get_AD();if(Value != new_value) {i++;if(i > FILTER_N) {i = 0;Value = new_value;}}elsei = 0;返回值;
}10.限制防跳过滤方式
/*
A.名称：限制防跳过滤法
B.方法：相当于“限制过滤法”+“跳动消除过滤法”；首先限制，然后消除抖动。
C、优点：继承了“限幅”和“防抖”的优点；改进“去抖滤波法”的一些缺陷，避免将干扰值引入系统。
D.缺点：不适合快速变化的参数。
E、主办单位：深海宇 2013-11-01
*/int Filter_Value;
int Value;void setup() {Serial.begin(9600); // 初始化串行通信 randomSeed(analogRead(0)); // 生成随机种子 Value = 300;}void 循环() {Filter_Value = Filter(); // 获取过滤器输出值 Serial.println(Filter_Value); // 串口输出延时(50);
}//用于随机生成300左右的当前值
int Get_AD() {返回随机数(295, 305);
}//限制和防跳过滤方法
#定义FILTER_A 1
#定义FILTER_N 5
整数 i = 0;
int Filter() {int NewValue;int new_value;NewValue = Get_AD();if(((NewValue - Value) > FILTER_A) || ((Value - NewValue) > FILTER_A))new_value = Value;elsenew_value = NewValue;if(值！= new_value) {i++;if(i > FILTER_N) {i = 0;Value = new_value;}}elsei = 0;返回值;
}11.卡尔曼滤波（非扩展卡尔曼）
#include // I2C 库、陀螺仪// 加速度计 ADXL345
#define ACC (0x53) //ADXL345 ACC地址
#define A_TO_READ (6) //每次要读取的字节数（每个轴两个字节）//陀螺仪ITG3200
#define GYRO 0x68 // 当 AD0 连接到 Vcc 时，陀螺仪地址，二进制 = 11101000（请参阅分线板的原理图）
#定义G_SMPLRT_DIV 0x15#定义G_DLPF_FS 0x16
#定义G_INT_CFG 0x17
#define G_PWR_MGM 0x3E#define G_TO_READ 8 // 每个轴 x、y、z 2 个字节 // 偏移量是芯片特定的。
int a_offx = 0;
int a_offy = 0;
int a_offz = 0;int g_offx = 0;
int g_offy = 0;
int g_offz = 0;char str[512]; void initAcc() {//打开ADXL345writeTo(ACC, 0x2D, 0); writeTo(ACC, 0x2D, 16);writeTo(ACC, 0x2D, 8);//默认设备在+-2g范围内读取
}void getAccelerometerData(int* 结果) {int regAddress = 0x32; //ADXL345byte 上的第一个轴加速度数据寄存器 buff[A_TO_READ];readFrom(ACC, regAddress, A_TO_READ, buff); //从ADXL345读取加速度数据//每个轴读数采用10位分辨率，即2个字节。最低有效字节在前！！//因此我们将两个字节转换为一个 intresult[0] = (((int)buff[1]) << 8) | buff[0] + a_offx;结果[1] = (((int)buff[3]) << 8) | buff[2] + a_offy;结果[2] = (((int)buff[5]) << 8) | buff[4] + a_offz;}//初始化陀螺仪
无效 initGyro()
{/******************************************** ITG 3200* 电源管理套件至：*时钟选择=内部振荡器*无复位，无睡眠模式*无待机模式*采样率= 125Hz*参数+/- 2000度/秒*低通滤波器= 5Hz*无中断****** **********************************/writeTo(陀螺仪, G_PWR_MGM, 0x00);writeTo(陀螺仪, G_SMPLRT_DIV，0x07）； // EB, 50, 80, 7F, DE, 23, 20, FFwriteTo(GYRO, G_DLPF_FS, 0x1E); // +/- 2000 dgrs/秒, 1KHz, 1E, 19writeTo(GYRO, G_INT_CFG, 0x00);
}void getGyrscopeData(int * 结果){/****************************************陀螺仪 ITG-3200 I2C 寄存器：临时 MSB = 1B ，温度LSB = 1Cx轴MSB = 1D，x轴LSB = 1Ey轴MSB = 1F，y轴LSB = 20z轴MSB = 21，z轴LSB = 22**************** **********************/int regAddress = 0x1B;int temp, x, y, z;byte buff[G_TO_READ];readFrom(GYRO, regAddress, G_TO_READ ，增益）； //从ITG3200读取陀螺仪数据result[0] = ((buff[2] << 8) | buff[3]) + g_offx;result[1] = ((buff[4] << 8) | buff[ 5]) + g_offy;结果[2] = ((buff[6] << 8) | buff[7]) + g_offz;结果[3] = (buff[0] << 8) |增益[1]； // 温度}float xz=0,yx=0,yz=0;
浮点数 p_xz=1,p_yx=1,p_yz=1;
浮点数 q_xz=0.0025,q_yx=0.0025,q_yz=0.0025;
浮点数 k_xz=0,k_yx=0,k_yz=0;
float r_xz=0.25,r_yx=0.25,r_yz=0.25;//int acc_temp[3];//float acc[3];int acc[3];int gyro[4];float Axz;float Ayx;float Ayz;浮动t=0.025；
无效设置()
{Serial.begin(9600);Wire.begin();initAcc();initGyro();}//无符号长定时器 = 0;
//浮动o；
无效循环(){getAccelerometerData(acc);getGyroscopyData(gyro);//timer = millis();sprintf(str, "%d,%d,%d,%d,%d,%d", acc[0],acc[ 1],acc[2],陀螺仪[0],陀螺仪[1],陀螺仪[2]);//acc[0]=acc[0];//acc[2]=acc[2];// acc[1]=acc[1];//r=sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);陀螺仪[0 ]=陀螺仪[0]/ 14.375;陀螺仪[1]=陀螺仪[1]/ (-14.375);陀螺仪[2]=陀螺仪[2]/ 14.375;Axz=(atan2(acc[0],acc[2] ))*180/PI;Ayx=(atan2(acc[0],acc[1]))*180/PI;/*if((acc[0]!=0)&&(acc[1]!=0 )){Ayx=(atan2(acc[0],acc[1]))*180/PI;}else{Ayx=t*陀螺仪[2];}*/Ayz=(atan2(acc[1],acc [2]))*180/PI;//卡尔曼滤波器calculate_xz();calculate_yx();calculate_yz();//sprintf(str, "%d,%d,%d", xz_1, xy_1, x_1);/ /Serial.print(xz);Serial.print(",");//Serial.print(yx);Serial.print(",");//Serial.print(yz);Serial.print(", ");//sprintf(str, "%d,%d,%d,%d,%d,%d", acc[0],acc[1],acc[2],陀螺仪[0],陀螺仪[1],陀螺仪[2]);//sprintf(str, "%d,%d,%d",陀螺仪[0],陀螺仪[1],陀螺仪[2]);Serial.print(Axz); Serial.print(",");//Serial.print(Ayx);Serial.print(",");//Serial.print(Ayz);Serial.print(",");//Serial.print(str);//o=gyro[2];//w=acc[2];//Serial.print(o);Serial.print(" ,");//Serial.print(w);Serial.print(",");Serial.print("\n");//延迟(50);}
无效计算_xz()
{xz=xz+t*陀螺仪[1];
p_xz=p_xz+q_xz;
k_xz=p_xz/(p_xz+r_xz);
xz=xz+k_xz*(Axz-xz);
p_xz=(1-k_xz)*p_xz;
}
无效计算_yx()
{yx=yx+t*陀螺仪[2];p_yx=p_yx+q_yx;k_yx=p_yx/(p_yx+r_yx);yx=yx+k_yx*(Ayx-yx);p_yx=(1-k_yx)*p_yx; }
无效计算_yz()
{yz=yz+t*陀螺仪[0];p_yz=p_yz+q_yz;k_yz=p_yz/(p_yz+r_yz);yz=yz+k_yz*(Ayz-yz);p_yz=(1-k_yz)*p_yz; }// - - - - - - - - 功能
//将val写入ACC上的地址寄存器
void writeTo(int DEVICE, 字节地址, 字节值) {Wire.beginTransmission(DEVICE); //开始传输到ACC Wire.write(address); // 发送寄存器addressWire.write(val); // 发送值到 writeWire.endTransmission(); //结束传输
}//从 ACC 上的地址寄存器开始读取 num 个字节到 buff 数组中void readFrom(int DEVICE, 字节地址, int num, byte buff[]) {Wire.beginTransmission(DEVICE); //开始传输到ACC Wire.write(address); //发送地址读取 fromWire.endTransmission(); //结束传输Wire.beginTransmission(DEVICE); //开始传输到ACCWire.requestFrom(DEVICE, num); // 从 ACC 请求 6 个字节 i = 0;while(Wire.available()) //ACC 可能发送少于请求的字节（异常）{ buff[i] = www.405452.com(); // 接收一个 bytei++;}Wire.endTransmission(); //结束传输
} 