十大滤波算法程序大全(Arduino精编无错版)

1、本帖最后由 shenhaiyu 于 2013-11-11 23:03 编辑最近用Arduino做电子秤，为了解决数据的跳变研究了不少滤波算法。网上能找到大把的十大滤波算法帖子，每一篇都不太一样，都号称精编啊，除错啊什么的，可是放到板子里却没一个能正常跑起来的。于是决定自己整理一下这些程序，完美移植到Arduino中。所以大家看到这个帖子的时候，不要怀疑我重复发帖。我的代码都是经过反复试验，复制到Arduino中就能开跑的成品代码，移植到自己的程序中非常方便。而且都仔细研究了各个算法，把错误都修正了的（别的程序连冒泡算法都是溢出的，不信自己找来细看看），所以也算个小原创吧，在别人基础上的原创。转

2、载请注明出处：极客工坊/thread-7694-1-1.htmlBy shenhaiyu 2013-11-011、限幅滤波法（又称程序判断滤波法）2、中位值滤波法3、算术平均滤波法4、递推平均滤波法（又称滑动平均滤波法）5、中位值平均滤波法（又称防脉冲干扰平均滤波法）6、限幅平均滤波法7、一阶滞后滤波法8、加权递推平均滤波法9、消抖滤波法10、限幅消抖滤波法11、新增加 卡尔曼滤波（非扩展卡尔曼），代码在17楼（点击这里）感谢zhangzhe0617分享程序默认对int类型数据进行滤波，如需要对其他类型进行滤波，只需要把程序中所有int替换

3、成long、float或者double即可。1、限幅滤波法（又称程序判断滤波法）ARDUINO 代码复制打印1. /*2. A、名称：限幅滤波法（又称程序判断滤波法）3. B、方法：4. 根据经验判断，确定两次采样允许的最大偏差值（设为A），5. 每次检测到新值时判断：6. 如果本次值与上次值之差A，则本次值无效，放弃本次值，用上次值代替本次值。8. C、优点：9. 能有效克服因偶然因素引起的脉冲干扰。10. D、缺点：11. 无法抑制那种周期性的干扰。12. 平滑度差。13. E、整理：shenhaiyu 2013-11-0114. */15. 16. int Filter_Value;17

4、. int Value;18. 19. void setup() 20. Serial.begin(9600); / 初始化串口通信21. randomSeed(analogRead(0); / 产生随机种子22. Value = 300;23. 24. 25. void loop() 26. Filter_Value = Filter(); / 获得滤波器输出值27. Value = Filter_Value; / 最近一次有效采样的值，该变量为全局变量28. Serial.println(Filter_Value); / 串口输出29. delay(50);30. 31. 32. / 用于

5、随机产生一个300左右的当前值33. int Get_AD() 34. return random(295, 305);35. 36. 37. / 限幅滤波法（又称程序判断滤波法）38. #define FILTER_A 139. int Filter() 40. int NewValue;41. NewValue = Get_AD();42. if(NewValue - Value) FILTER_A) | (Value - NewValue) FILTER_A)43. return Value;44. else45. return NewValue;46. 2、中位值滤波法ARDUINO

6、代码复制打印1. /*2. A、名称：中位值滤波法3. B、方法：4. 连续采样N次（N取奇数），把N次采样值按大小排列，5. 取中间值为本次有效值。6. C、优点：7. 能有效克服因偶然因素引起的波动干扰；8. 对温度、液位的变化缓慢的被测参数有良好的滤波效果。9. D、缺点：10. 对流量、速度等快速变化的参数不宜。11. E、整理：shenhaiyu 2013-11-0112. */13. 14. int Filter_Value;15. 16. void setup() 17. Serial.begin(9600); / 初始化串口通信18. randomSeed(analogRead

7、(0); / 产生随机种子19. 20. 21. void loop() 22. Filter_Value = Filter(); / 获得滤波器输出值23. Serial.println(Filter_Value); / 串口输出24. delay(50);25. 26. 27. / 用于随机产生一个300左右的当前值28. int Get_AD() 29. return random(295, 305);30. 31. 32. / 中位值滤波法33. #define FILTER_N 10134. int Filter() 35. int filter_bufFILTER_N;36. in

8、t i, j;37. int filter_temp;38. for(i = 0; i FILTER_N; i+) 39. filter_bufi = Get_AD();40. delay(1);41. 42. / 采样值从小到大排列（冒泡法）43. for(j = 0; j FILTER_N - 1; j+) 44. for(i = 0; i filter_bufi + 1) 46. filter_temp = filter_bufi;47. filter_bufi = filter_bufi + 1;48. filter_bufi + 1 = filter_temp;49. 50. 51.

9、 52. return filter_buf(FILTER_N - 1) / 2;53. 3、算术平均滤波法ARDUINO 代码复制打印1. /*2. A、名称：算术平均滤波法3. B、方法：4. 连续取N个采样值进行算术平均运算：5. N值较大时：信号平滑度较高，但灵敏度较低；6. N值较小时：信号平滑度较低，但灵敏度较高；7. N值的选取：一般流量，N=12；压力：N=4。8. C、优点：9. 适用于对一般具有随机干扰的信号进行滤波；10. 这种信号的特点是有一个平均值，信号在某一数值范围附近上下波动。11. D、缺点：12. 对于测量速度较慢或要求数据计算速度较快的实时控制不适用；13.

10、 比较浪费RAM。14. E、整理：shenhaiyu 2013-11-0115. */16. 17. int Filter_Value;18. 19. void setup() 20. Serial.begin(9600); / 初始化串口通信21. randomSeed(analogRead(0); / 产生随机种子22. 23. 24. void loop() 25. Filter_Value = Filter(); / 获得滤波器输出值26. Serial.println(Filter_Value); / 串口输出27. delay(50);28. 29. 30. / 用于随机产生一个

11、300左右的当前值31. int Get_AD() 32. return random(295, 305);33. 34. 35. / 算术平均滤波法36. #define FILTER_N 1237. int Filter() 38. int i;39. int filter_sum = 0;40. for(i = 0; i FILTER_N; i+) 41. filter_sum += Get_AD();42. delay(1);43. 44. return (int)(filter_sum / FILTER_N);45. 4、递推平均滤波法（又称滑动平均滤波法）ARDUINO 代码复制打

12、印1. /*2. A、名称：递推平均滤波法（又称滑动平均滤波法）3. B、方法：4. 把连续取得的N个采样值看成一个队列，队列的长度固定为N，5. 每次采样到一个新数据放入队尾，并扔掉原来队首的一次数据（先进先出原则），6. 把队列中的N个数据进行算术平均运算，获得新的滤波结果。7. N值的选取：流量，N=12；压力，N=4；液面，N=4-12；温度，N=1-4。8. C、优点：9. 对周期性干扰有良好的抑制作用，平滑度高；10. 适用于高频振荡的系统。11. D、缺点：12. 灵敏度低，对偶然出现的脉冲性干扰的抑制作用较差；13. 不易消除由于脉冲干扰所引起的采样值偏差；14. 不适用于脉冲

13、干扰比较严重的场合；15. 比较浪费RAM。16. E、整理：shenhaiyu 2013-11-0117. */18. 19. int Filter_Value;20. 21. void setup() 22. Serial.begin(9600); / 初始化串口通信23. randomSeed(analogRead(0); / 产生随机种子24. 25. 26. void loop() 27. Filter_Value = Filter(); / 获得滤波器输出值28. Serial.println(Filter_Value); / 串口输出29. delay(50);30. 31. 3

14、2. / 用于随机产生一个300左右的当前值33. int Get_AD() 34. return random(295, 305);35. 36. 37. / 递推平均滤波法（又称滑动平均滤波法）38. #define FILTER_N 1239. int filter_bufFILTER_N + 1;40. int Filter() 41. int i;42. int filter_sum = 0;43. filter_bufFILTER_N = Get_AD();44. for(i = 0; i FILTER_N; i+) 45. filter_bufi = filter_bufi +

15、1; / 所有数据左移，低位仍掉46. filter_sum += filter_bufi;47. 48. return (int)(filter_sum / FILTER_N);49. 5、中位值平均滤波法（又称防脉冲干扰平均滤波法）ARDUINO 代码复制打印1. /*2. A、名称：中位值平均滤波法（又称防脉冲干扰平均滤波法）3. B、方法：4. 采一组队列去掉最大值和最小值后取平均值，5. 相当于“中位值滤波法”+“算术平均滤波法”。6. 连续采样N个数据，去掉一个最大值和一个最小值，7. 然后计算N-2个数据的算术平均值。8. N值的选取：3-14。9. C、优点：10. 融合了“中

16、位值滤波法”+“算术平均滤波法”两种滤波法的优点。11. 对于偶然出现的脉冲性干扰，可消除由其所引起的采样值偏差。12. 对周期干扰有良好的抑制作用。13. 平滑度高，适于高频振荡的系统。14. D、缺点：15. 计算速度较慢，和算术平均滤波法一样。16. 比较浪费RAM。17. E、整理：shenhaiyu 2013-11-0118. */19. 20. int Filter_Value;21. 22. void setup() 23. Serial.begin(9600); / 初始化串口通信24. randomSeed(analogRead(0); / 产生随机种子25. 26. 27.

17、 void loop() 28. Filter_Value = Filter(); / 获得滤波器输出值29. Serial.println(Filter_Value); / 串口输出30. delay(50);31. 32. 33. / 用于随机产生一个300左右的当前值34. int Get_AD() 35. return random(295, 305);36. 37. 38. / 中位值平均滤波法（又称防脉冲干扰平均滤波法）（算法1）39. #define FILTER_N 10040. int Filter() 41. int i, j;42. int filter_temp, fi

18、lter_sum = 0;43. int filter_bufFILTER_N;44. for(i = 0; i FILTER_N; i+) 45. filter_bufi = Get_AD();46. delay(1);47. 48. / 采样值从小到大排列（冒泡法）49. for(j = 0; j FILTER_N - 1; j+) 50. for(i = 0; i filter_bufi + 1) 52. filter_temp = filter_bufi;53. filter_bufi = filter_bufi + 1;54. filter_bufi + 1 = filter_tem

19、p;55. 56. 57. 58. / 去除最大最小极值后求平均59. for(i = 1; i FILTER_N - 1; i+) filter_sum += filter_bufi;60. return filter_sum / (FILTER_N - 2);61. 62. 63. 64. /中位值平均滤波法（又称防脉冲干扰平均滤波法）（算法2）65. /*66. #define FILTER_N 10067. int Filter() 68. int i;69. int filter_sum = 0;70. int filter_max, filter_min;71. int filte

20、r_bufFILTER_N;72. for(i = 0; i 0; i-) 80. if(filter_bufi filter_max)81. filter_max=filter_bufi;82. else if(filter_bufi FILTER_A) | (filter_bufFILTER_N - 2 - filter_bufFILTER_N - 1) FILTER_A)43. filter_bufFILTER_N - 1 = filter_bufFILTER_N - 2;44. for(i = 0; i FILTER_N - 1; i+) 45. filter_bufi = filte

21、r_bufi + 1;46. filter_sum += filter_bufi;47. 48. return (int)filter_sum / (FILTER_N - 1);49. 7、一阶滞后滤波法ARDUINO 代码复制打印1. /*2. A、名称：一阶滞后滤波法3. B、方法：4. 取a=0-1，本次滤波结果=(1-a)*本次采样值+a*上次滤波结果。5. C、优点：6. 对周期性干扰具有良好的抑制作用；7. 适用于波动频率较高的场合。8. D、缺点：9. 相位滞后，灵敏度低；10. 滞后程度取决于a值大小；11. 不能消除滤波频率高于采样频率1/2的干扰信号。12. E、整理：sh

22、enhaiyu 2013-11-0113. */14. 15. int Filter_Value;16. int Value;17. 18. void setup() 19. Serial.begin(9600); / 初始化串口通信20. randomSeed(analogRead(0); / 产生随机种子21. Value = 300;22. 23. 24. void loop() 25. Filter_Value = Filter(); / 获得滤波器输出值26. Serial.println(Filter_Value); / 串口输出27. delay(50);28. 29. 30.

23、/ 用于随机产生一个300左右的当前值31. int Get_AD() 32. return random(295, 305);33. 34. 35. / 一阶滞后滤波法36. #define FILTER_A 0.0137. int Filter() 38. int NewValue;39. NewValue = Get_AD();40. Value = (int)(float)NewValue * FILTER_A + (1.0 - FILTER_A) * (float)Value);41. return Value;42. 8、加权递推平均滤波法ARDUINO 代码复制打印1. /*2.

24、 A、名称：加权递推平均滤波法3. B、方法：4. 是对递推平均滤波法的改进，即不同时刻的数据加以不同的权；5. 通常是，越接近现时刻的数据，权取得越大。6. 给予新采样值的权系数越大，则灵敏度越高，但信号平滑度越低。7. C、优点：8. 适用于有较大纯滞后时间常数的对象，和采样周期较短的系统。9. D、缺点：10. 对于纯滞后时间常数较小、采样周期较长、变化缓慢的信号；11. 不能迅速反应系统当前所受干扰的严重程度，滤波效果差。12. E、整理：shenhaiyu 2013-11-0113. */14. 15. int Filter_Value;16. 17. void setup() 18

25、. Serial.begin(9600); / 初始化串口通信19. randomSeed(analogRead(0); / 产生随机种子20. 21. 22. void loop() 23. Filter_Value = Filter(); / 获得滤波器输出值24. Serial.println(Filter_Value); / 串口输出25. delay(50);26. 27. 28. / 用于随机产生一个300左右的当前值29. int Get_AD() 30. return random(295, 305);31. 32. 33. / 加权递推平均滤波法34. #define FIL

26、TER_N 1235. int coeFILTER_N = 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12; / 加权系数表36. int sum_coe = 1 + 2 + 3 + 4 + 5 + 6 + 7 + 8 + 9 + 10 + 11 + 12; / 加权系数和37. int filter_bufFILTER_N + 1;38. int Filter() 39. int i;40. int filter_sum = 0;41. filter_bufFILTER_N = Get_AD();42. for(i = 0; i FILTER_N; i+) 43.

27、 filter_bufi = filter_bufi + 1; / 所有数据左移，低位仍掉44. filter_sum += filter_bufi * coei;45. 46. filter_sum /= sum_coe;47. return filter_sum;48. 9、消抖滤波法ARDUINO 代码复制打印1. /*2. A、名称：消抖滤波法3. B、方法：4. 设置一个滤波计数器，将每次采样值与当前有效值比较：5. 如果采样值=当前有效值，则计数器清零；6. 如果采样值当前有效值，则计数器+1，并判断计数器是否=上限N（溢出）；7. 如果计数器溢出，则将本次值替换当前有效值，并清计

28、数器。8. C、优点：9. 对于变化缓慢的被测参数有较好的滤波效果；10. 可避免在临界值附近控制器的反复开/关跳动或显示器上数值抖动。11. D、缺点：12. 对于快速变化的参数不宜；13. 如果在计数器溢出的那一次采样到的值恰好是干扰值,则会将干扰值当作有效值导入系统。14. E、整理：shenhaiyu 2013-11-0115. */16. 17. int Filter_Value;18. int Value;19. 20. void setup() 21. Serial.begin(9600); / 初始化串口通信22. randomSeed(analogRead(0); / 产生随

29、机种子23. Value = 300;24. 25. 26. void loop() 27. Filter_Value = Filter(); / 获得滤波器输出值28. Serial.println(Filter_Value); / 串口输出29. delay(50);30. 31. 32. / 用于随机产生一个300左右的当前值33. int Get_AD() 34. return random(295, 305);35. 36. 37. / 消抖滤波法38. #define FILTER_N 1239. int i = 0;40. int Filter() 41. int new_val

30、ue;42. new_value = Get_AD();43. if(Value != new_value) 44. i+;45. if(i FILTER_N) 46. i = 0;47. Value = new_value;48. 49. 50. else51. i = 0;52. return Value;53. 10、限幅消抖滤波法ARDUINO 代码复制打印1. /*2. A、名称：限幅消抖滤波法3. B、方法：4. 相当于“限幅滤波法”+“消抖滤波法”；5. 先限幅，后消抖。6. C、优点：7. 继承了“限幅”和“消抖”的优点；8. 改进了“消抖滤波法”中的某些缺陷，避免将干扰值导入

31、系统。9. D、缺点：10. 对于快速变化的参数不宜。11. E、整理：shenhaiyu 2013-11-0112. */13. 14. int Filter_Value;15. int Value;16. 17. void setup() 18. Serial.begin(9600); / 初始化串口通信19. randomSeed(analogRead(0); / 产生随机种子20. Value = 300;21. 22. 23. void loop() 24. Filter_Value = Filter(); / 获得滤波器输出值25. Serial.println(Filter_Va

32、lue); / 串口输出26. delay(50);27. 28. 29. / 用于随机产生一个300左右的当前值30. int Get_AD() 31. return random(295, 305);32. 33. 34. / 限幅消抖滤波法35. #define FILTER_A 136. #define FILTER_N 537. int i = 0;38. int Filter() 39. int NewValue;40. int new_value;41. NewValue = Get_AD();42. if(NewValue - Value) FILTER_A) | (Value

33、 - NewValue) FILTER_A)43. new_value = Value;44. else45. new_value = NewValue;46. if(Value != new_value) 47. i+;48. if(i FILTER_N) 49. i = 0;50. Value = new_value;51. 52. 53. else54. i = 0;55. return Value;56. 建议编辑一下这个帖子作为滤波专用的，这样大家查起来也方便。下面是卡尔曼滤波，不是扩展的，但是输出平稳的俯仰和滚转应该够了(凑乎用吧我也不是专业写代码的，欢迎大家拍)1. #inclu

34、de / I2C library, gyroscope2.3. / Accelerometer ADXL3454. #define ACC (0x53) /ADXL345 ACC address5. #define A_TO_READ (6) /num of bytes we are going to read each time (two bytes for each axis)6.7.8. / Gyroscope ITG32009. #define GYRO 0x68 / gyro address, binary = when AD0 is connected to Vcc (see sc

35、hematics of your breakout board)10. #define G_SMPLRT_DIV 0x15 11. #define G_DLPF_FS 0x16 12. #define G_INT_CFG 0x1713. #define G_PWR_MGM 0x3E14.15. #define G_TO_READ 8 / 2 bytes for each axis x, y, z16.17.18. / offsets are chip specific.19. int a_offx = 0;20. int a_offy = 0;21. int a_offz = 0;22.23.

36、 int g_offx = 0;24. int g_offy = 0;25. int g_offz = 0;26. /27.28. /29. char str512;30.31. void initAcc() 32. /Turning on the ADXL34533. writeTo(ACC, 0x2D, 0); 34. writeTo(ACC, 0x2D, 16);35. writeTo(ACC, 0x2D, 8);36. /by default the device is in +-2g range reading37. 38.39. void getAccelerometerData(

37、int* result) 40. int regAddress = 0x32; /first axis-acceleration-data register on the ADXL34541. byte buffA_TO_READ;42. 43. readFrom(ACC, regAddress, A_TO_READ, buff); /read the acceleration data from the ADXL34544. 45. /each axis reading comes in 10 bit resolution, ie 2 bytes.Least Significat Byte

38、first!46. /thus we are converting both bytes in to one int47. result0 = (int)buff1) 8) | buff0 + a_offx; 48. result1 = (int)buff3) 8) | buff2 + a_offy;49. result2 = (int)buff5) 8) | buff4 + a_offz;50. 51.52. /initializes the gyroscope53. void initGyro()54. 55. /*56. * ITG 320057. * power management

39、set to:58. * clock select = internal oscillator59. * no reset, no sleep mode60. * no standby mode61. * sample rate to = 125Hz62. * parameter to +/- 2000 degrees/sec63. * low pass filter = 5Hz64. * no interrupt65. */66. writeTo(GYRO, G_PWR_MGM, 0x00);67. writeTo(GYRO, G_SMPLRT_DIV, 0x07); / EB, 50, 8

40、0, 7F, DE, 23, 20, FF68. writeTo(GYRO, G_DLPF_FS, 0x1E); / +/- 2000 dgrs/sec, 1KHz, 1E, 1969. writeTo(GYRO, G_INT_CFG, 0x00);70. 71.72.73. void getGyroscopeData(int * result)74. 75. /*76. Gyro ITG-3200 I2C77. registers:78. temp MSB = 1B, temp LSB = 1C79. x axis MSB = 1D, x axis LSB = 1E80. y axis MS

41、B = 1F, y axis LSB = 2081. z axis MSB = 21, z axis LSB = 2282. */83.84. int regAddress = 0x1B;85. int temp, x, y, z;86. byte buffG_TO_READ;87. 88. readFrom(GYRO, regAddress, G_TO_READ, buff); /read the gyro data from the ITG320089. 90. result0 = (buff2 8) | buff3) + g_offx;91. result1 = (buff4 8) |

42、buff5) + g_offy;92. result2 = (buff6 8) | buff7) + g_offz;93. result3 = (buff0 8) | buff1; / temperature94. 95. 96.97.98. float xz=0,yx=0,yz=0;99. float p_xz=1,p_yx=1,p_yz=1;100. float q_xz=0.0025,q_yx=0.0025,q_yz=0.0025;101. float k_xz=0,k_yx=0,k_yz=0;102. float r_xz=0.25,r_yx=0.25,r_yz=0.25;103. /

43、int acc_temp3;104. /float acc3;105. int acc3;106. int gyro4;107. float Axz;108. float Ayx;109. float Ayz;110. float t=0.025;111. void setup()112. 113. Serial.begin(9600);114. Wire.begin();115. initAcc();116. initGyro();117. 118. 119.120. /unsigned long timer = 0;121. /float o;122. void loop()123. 12

44、4. 125. getAccelerometerData(acc);126. getGyroscopeData(gyro);127. /timer = millis();128. sprintf(str, %d,%d,%d,%d,%d,%d, acc0,acc1,acc2,gyro0,gyro1,gyro2);129. 130. /acc0=acc0;131. /acc2=acc2;132. /acc1=acc1;133. /r=sqrt(acc0*acc0+acc1*acc1+acc2*acc2);134. gyro0=gyro0/ 14.375;135. gyro1=gyro1/ (-14.375);136. gyro2=gyro2/ 14.375;137. 138. 139. Axz=(atan2(acc0