首页 > 编程语言 >11种常用滤波算法程序

11种常用滤波算法程序

时间:2023-09-03 12:33:48浏览次数:62  
标签:11 FILTER int 滤波 Value filter 算法 Filter

来源:嵌入式情报局

一、限幅滤波法(程序判断滤波法)

 1 /*
 2 A、名称:限幅滤波法(又称程序判断滤波法)
 3 B、方法:
 4     根据经验判断,确定两次采样允许的最大偏差值(设为A),
 5     每次检测到新值时判断:
 6     如果本次值与上次值之差<=A,则本次值有效,
 7     如果本次值与上次值之差>A,则本次值无效,放弃本次值,用上次值代替本次值。
 8 C、优点:
 9     能有效克服因偶然因素引起的脉冲干扰。
10 D、缺点:
11     无法抑制那种周期性的干扰。
12     平滑度差。
13 E、整理:shenhaiyu 2013-11-01
14 */
15 
16 int Filter_Value;
17 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 // 用于随机产生一个300左右的当前值
33 int Get_AD() {
34   return random(295, 305);
35 }
36 
37 // 限幅滤波法(又称程序判断滤波法)
38 #define FILTER_A 1
39 int Filter() {
40   int NewValue;
41   NewValue = Get_AD();
42   if(((NewValue - Value) > FILTER_A) || ((Value - NewValue) > FILTER_A))
43     return Value;
44   else
45     return NewValue;
46 }

二、中值滤波法

 1 /*
 2 A、名称:中位值滤波法
 3 B、方法:
 4     连续采样N次(N取奇数),把N次采样值按大小排列,
 5     取中间值为本次有效值。
 6 C、优点:
 7     能有效克服因偶然因素引起的波动干扰;
 8     对温度、液位的变化缓慢的被测参数有良好的滤波效果。
 9 D、缺点:
10     对流量、速度等快速变化的参数不宜。
11 E、整理:shenhaiyu 2013-11-01
12 */
13 
14 int Filter_Value;
15 
16 void setup() {
17   Serial.begin(9600);       // 初始化串口通信
18   randomSeed(analogRead(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 101
34 int Filter() {
35   int filter_buf[FILTER_N];
36   int i, j;
37   int filter_temp;
38   for(i = 0; i < FILTER_N; i++) {
39     filter_buf[i] = Get_AD();
40     delay(1);
41   }
42   // 采样值从小到大排列(冒泡法)
43   for(j = 0; j < FILTER_N - 1; j++) {
44     for(i = 0; i < FILTER_N - 1 - j; i++) {
45       if(filter_buf[i] > filter_buf[i + 1]) {
46         filter_temp = filter_buf[i];
47         filter_buf[i] = filter_buf[i + 1];
48         filter_buf[i + 1] = filter_temp;
49       }
50     }
51   }
52   return filter_buf[(FILTER_N - 1) / 2];
53 }

三、算术平均滤波法

 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     比较浪费RAM。
14 E、整理:shenhaiyu 2013-11-01
15 */
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 // 用于随机产生一个300左右的当前值
31 int Get_AD() {
32   return random(295, 305);
33 }
34 
35 // 算术平均滤波法
36 #define FILTER_N 12
37 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 }

四、递推平均滤波法(又称滑动平均滤波法)

 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     不适用于脉冲干扰比较严重的场合;
15     比较浪费RAM。
16 E、整理:shenhaiyu 2013-11-01
17 */
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 
32 // 用于随机产生一个300左右的当前值
33 int Get_AD() {
34   return random(295, 305);
35 }
36 
37 // 递推平均滤波法(又称滑动平均滤波法)
38 #define FILTER_N 12
39 int filter_buf[FILTER_N + 1];
40 int Filter() {
41   int i;
42   int filter_sum = 0;
43   filter_buf[FILTER_N] = Get_AD();
44   for(i = 0; i < FILTER_N; i++) {
45     filter_buf[i] = filter_buf[i + 1]; // 所有数据左移,低位仍掉
46     filter_sum += filter_buf[i];
47   }
48   return (int)(filter_sum / FILTER_N);
49 }

五、中位值平均滤波法(又称防脉冲干扰平均滤波法)

 1 /*
 2 A、名称:中位值平均滤波法(又称防脉冲干扰平均滤波法)
 3 B、方法:
 4     采一组队列去掉最大值和最小值后取平均值,
 5     相当于“中位值滤波法”+“算术平均滤波法”。
 6     连续采样N个数据,去掉一个最大值和一个最小值,
 7     然后计算N-2个数据的算术平均值。
 8     N值的选取:3-14。
 9 C、优点:
10     融合了“中位值滤波法”+“算术平均滤波法”两种滤波法的优点。
11     对于偶然出现的脉冲性干扰,可消除由其所引起的采样值偏差。
12     对周期干扰有良好的抑制作用。
13     平滑度高,适于高频振荡的系统。
14 D、缺点:
15     计算速度较慢,和算术平均滤波法一样。
16     比较浪费RAM。
17 E、整理:shenhaiyu 2013-11-01
18 */
19 
20 int Filter_Value;
21 
22 void setup() {
23   Serial.begin(9600);       // 初始化串口通信
24   randomSeed(analogRead(0)); // 产生随机种子
25 }
26 
27 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 100
40 int Filter() {
41   int i, j;
42   int filter_temp, filter_sum = 0;
43   int filter_buf[FILTER_N];
44   for(i = 0; i < FILTER_N; i++) {
45     filter_buf[i] = Get_AD();
46     delay(1);
47   }
48   // 采样值从小到大排列(冒泡法)
49   for(j = 0; j < FILTER_N - 1; j++) {
50     for(i = 0; i < FILTER_N - 1 - j; i++) {
51       if(filter_buf[i] > filter_buf[i + 1]) {
52         filter_temp = filter_buf[i];
53         filter_buf[i] = filter_buf[i + 1];
54         filter_buf[i + 1] = filter_temp;
55       }
56     }
57   }
58   // 去除最大最小极值后求平均
59   for(i = 1; i < FILTER_N - 1; i++) filter_sum += filter_buf[i];
60   return filter_sum / (FILTER_N - 2);
61 }
62 
63 
64 //  中位值平均滤波法(又称防脉冲干扰平均滤波法)(算法2)
65 /*
66 #define FILTER_N 100
67 int Filter() {
68   int i;
69   int filter_sum = 0;
70   int filter_max, filter_min;
71   int filter_buf[FILTER_N];
72   for(i = 0; i < FILTER_N; i++) {
73     filter_buf[i] = Get_AD();
74     delay(1);
75   }
76   filter_max = filter_buf[0];
77   filter_min = filter_buf[0];
78   filter_sum = filter_buf[0];
79   for(i = FILTER_N - 1; i > 0; i--) {
80     if(filter_buf[i] > filter_max)
81       filter_max=filter_buf[i];
82     else if(filter_buf[i] < filter_min)
83       filter_min=filter_buf[i];
84     filter_sum = filter_sum + filter_buf[i];
85     filter_buf[i] = filter_buf[i - 1];
86   }
87   i = FILTER_N - 2;
88   filter_sum = filter_sum - filter_max - filter_min + i / 2; // +i/2 的目的是为了四舍五入
89   filter_sum = filter_sum / i;
90   return filter_sum;
91 }*/

六、限幅平均滤波法

 1 /*
 2 A、名称:限幅平均滤波法
 3 B、方法:
 4     相当于“限幅滤波法”+“递推平均滤波法”;
 5     每次采样到的新数据先进行限幅处理,
 6     再送入队列进行递推平均滤波处理。
 7 C、优点:
 8     融合了两种滤波法的优点;
 9     对于偶然出现的脉冲性干扰,可消除由于脉冲干扰所引起的采样值偏差。
10 D、缺点:
11     比较浪费RAM。
12 E、整理:shenhaiyu 2013-11-01
13 */
14 
15 #define FILTER_N 12
16 int Filter_Value;
17 int filter_buf[FILTER_N];
18 
19 void setup() {
20   Serial.begin(9600);       // 初始化串口通信
21   randomSeed(analogRead(0)); // 产生随机种子
22   filter_buf[FILTER_N - 2] = 300;
23 }
24 
25 void loop() {
26   Filter_Value = Filter();       // 获得滤波器输出值
27   Serial.println(Filter_Value); // 串口输出
28   delay(50);
29 }
30 
31 // 用于随机产生一个300左右的当前值
32 int Get_AD() {
33   return random(295, 305);
34 }
35 
36 // 限幅平均滤波法
37 #define FILTER_A 1
38 int Filter() {
39   int i;
40   int filter_sum = 0;
41   filter_buf[FILTER_N - 1] = Get_AD();
42   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))
43     filter_buf[FILTER_N - 1] = filter_buf[FILTER_N - 2];
44   for(i = 0; i < FILTER_N - 1; i++) {
45     filter_buf[i] = filter_buf[i + 1];
46     filter_sum += filter_buf[i];
47   }
48   return (int)filter_sum / (FILTER_N - 1);
49 }

七、一阶滞后滤波法

 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、整理:shenhaiyu 2013-11-01
13 */
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 // 用于随机产生一个300左右的当前值
31 int Get_AD() {
32   return random(295, 305);
33 }
34 
35 // 一阶滞后滤波法
36 #define FILTER_A 0.01
37 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 }

八、加权递推平均滤波法

 1 /*
 2 A、名称:加权递推平均滤波法
 3 B、方法:
 4     是对递推平均滤波法的改进,即不同时刻的数据加以不同的权;
 5     通常是,越接近现时刻的数据,权取得越大。
 6     给予新采样值的权系数越大,则灵敏度越高,但信号平滑度越低。
 7 C、优点:
 8     适用于有较大纯滞后时间常数的对象,和采样周期较短的系统。
 9 D、缺点:
10     对于纯滞后时间常数较小、采样周期较长、变化缓慢的信号;
11     不能迅速反应系统当前所受干扰的严重程度,滤波效果差。
12 E、整理:shenhaiyu 2013-11-01
13 */
14 
15 int Filter_Value;
16 
17 void setup() {
18   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 FILTER_N 12
35 int coe[FILTER_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_buf[FILTER_N + 1];
38 int Filter() {
39   int i;
40   int filter_sum = 0;
41   filter_buf[FILTER_N] = Get_AD();
42   for(i = 0; i < FILTER_N; i++) {
43     filter_buf[i] = filter_buf[i + 1]; // 所有数据左移,低位仍掉
44     filter_sum += filter_buf[i] * coe[i];
45   }
46   filter_sum /= sum_coe;
47   return filter_sum;
48 }

九、消抖滤波法

 1 /*
 2 A、名称:消抖滤波法
 3 B、方法:
 4     设置一个滤波计数器,将每次采样值与当前有效值比较:
 5     如果采样值=当前有效值,则计数器清零;
 6     如果采样值<>当前有效值,则计数器+1,并判断计数器是否>=上限N(溢出);
 7     如果计数器溢出,则将本次值替换当前有效值,并清计数器。
 8 C、优点:
 9     对于变化缓慢的被测参数有较好的滤波效果;
10     可避免在临界值附近控制器的反复开/关跳动或显示器上数值抖动。
11 D、缺点:
12     对于快速变化的参数不宜;
13     如果在计数器溢出的那一次采样到的值恰好是干扰值,则会将干扰值当作有效值导入系统。
14 E、整理:shenhaiyu 2013-11-01
15 */
16 
17 int Filter_Value;
18 int Value;
19 
20 void setup() {
21   Serial.begin(9600);       // 初始化串口通信
22   randomSeed(analogRead(0)); // 产生随机种子
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 12
39 int i = 0;
40 int Filter() {
41   int new_value;
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   else
51     i = 0;
52   return Value;
53 }

十、限幅消抖滤波法

 1 /*
 2 A、名称:限幅消抖滤波法
 3 B、方法:
 4     相当于“限幅滤波法”+“消抖滤波法”;
 5     先限幅,后消抖。
 6 C、优点:
 7     继承了“限幅”和“消抖”的优点;
 8     改进了“消抖滤波法”中的某些缺陷,避免将干扰值导入系统。
 9 D、缺点:
10     对于快速变化的参数不宜。
11 E、整理:shenhaiyu 2013-11-01
12 */
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_Value); // 串口输出
26   delay(50);
27 }
28 
29 // 用于随机产生一个300左右的当前值
30 int Get_AD() {
31   return random(295, 305);
32 }
33 
34 // 限幅消抖滤波法
35 #define FILTER_A 1
36 #define FILTER_N 5
37 int i = 0;
38 int Filter() {
39   int NewValue;
40   int new_value;
41   NewValue = Get_AD();
42   if(((NewValue - Value) > FILTER_A) || ((Value - NewValue) > FILTER_A))
43     new_value = Value;
44   else
45     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   else
54     i = 0;
55   return Value;
56 }

十一、卡尔曼滤波算法(非扩展卡尔曼)

  1 #include <Wire.h> // I2C library, gyroscope
  2 
  3 // Accelerometer ADXL345
  4 #define ACC (0x53)    //ADXL345 ACC address
  5 #define A_TO_READ (6)        //num of bytes we are going to read each time (two bytes for each axis)
  6 
  7 
  8 // Gyroscope ITG3200 
  9 #define GYRO 0x68 // gyro address, binary = 11101000 when AD0 is connected to Vcc (see schematics of your breakout board)
 10 #define G_SMPLRT_DIV 0x15   
 11 #define G_DLPF_FS 0x16   
 12 #define G_INT_CFG 0x17
 13 #define G_PWR_MGM 0x3E
 14 
 15 #define G_TO_READ 8 // 2 bytes for each axis x, y, z
 16 
 17 
 18 // offsets are chip specific. 
 19 int a_offx = 0;
 20 int a_offy = 0;
 21 int a_offz = 0;
 22 
 23 int g_offx = 0;
 24 int g_offy = 0;
 25 int g_offz = 0;
 26 ////////////////////////
 27 
 28 ////////////////////////
 29 char str[512]; 
 30 
 31 void initAcc() {
 32   //Turning on the ADXL345
 33   writeTo(ACC, 0x2D, 0);      
 34   writeTo(ACC, 0x2D, 16);
 35   writeTo(ACC, 0x2D, 8);
 36   //by default the device is in +-2g range reading
 37 }
 38 
 39 void getAccelerometerData(int* result) {
 40   int regAddress = 0x32;    //first axis-acceleration-data register on the ADXL345
 41   byte buff[A_TO_READ];
 42   
 43   readFrom(ACC, regAddress, A_TO_READ, buff); //read the acceleration data from the ADXL345
 44   
 45   //each axis reading comes in 10 bit resolution, ie 2 bytes.  Least Significat Byte first!!
 46   //thus we are converting both bytes in to one int
 47   result[0] = (((int)buff[1]) << 8) | buff[0] + a_offx;   
 48   result[1] = (((int)buff[3]) << 8) | buff[2] + a_offy;
 49   result[2] = (((int)buff[5]) << 8) | buff[4] + a_offz;
 50 }
 51 
 52 //initializes the gyroscope
 53 void initGyro()
 54 {
 55   /*****************************************
 56   * ITG 3200
 57   * power management set to:
 58   * clock select = internal oscillator
 59   *     no reset, no sleep mode
 60   *   no standby mode
 61   * sample rate to = 125Hz
 62   * parameter to +/- 2000 degrees/sec
 63   * low pass filter = 5Hz
 64   * no interrupt
 65   ******************************************/
 66   writeTo(GYRO, G_PWR_MGM, 0x00);
 67   writeTo(GYRO, G_SMPLRT_DIV, 0x07); // EB, 50, 80, 7F, DE, 23, 20, FF
 68   writeTo(GYRO, G_DLPF_FS, 0x1E); // +/- 2000 dgrs/sec, 1KHz, 1E, 19
 69   writeTo(GYRO, G_INT_CFG, 0x00);
 70 }
 71 
 72 
 73 void getGyroscopeData(int * result)
 74 {
 75   /**************************************
 76   Gyro ITG-3200 I2C
 77   registers:
 78   temp MSB = 1B, temp LSB = 1C
 79   x axis MSB = 1D, x axis LSB = 1E
 80   y axis MSB = 1F, y axis LSB = 20
 81   z axis MSB = 21, z axis LSB = 22
 82   *************************************/
 83 
 84   int regAddress = 0x1B;
 85   int temp, x, y, z;
 86   byte buff[G_TO_READ];
 87   
 88   readFrom(GYRO, regAddress, G_TO_READ, buff); //read the gyro data from the ITG3200
 89   
 90   result[0] = ((buff[2] << 8) | buff[3]) + g_offx;
 91   result[1] = ((buff[4] << 8) | buff[5]) + g_offy;
 92   result[2] = ((buff[6] << 8) | buff[7]) + g_offz;
 93   result[3] = (buff[0] << 8) | buff[1]; // temperature
 94   
 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   //int acc_temp[3];
104   //float acc[3];
105   int acc[3];
106   int gyro[4];
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 {
124   
125   getAccelerometerData(acc);
126   getGyroscopeData(gyro);
127   //timer = millis();
128   sprintf(str, "%d,%d,%d,%d,%d,%d", acc[0],acc[1],acc[2],gyro[0],gyro[1],gyro[2]);
129   
130   //acc[0]=acc[0];
131   //acc[2]=acc[2];
132   //acc[1]=acc[1];
133   //r=sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
134   gyro[0]=gyro[0]/ 14.375;
135   gyro[1]=gyro[1]/ (-14.375);
136   gyro[2]=gyro[2]/ 14.375;
137   
138    
139   Axz=(atan2(acc[0],acc[2]))*180/PI;
140   Ayx=(atan2(acc[0],acc[1]))*180/PI;
141   /*if((acc[0]!=0)&&(acc[1]!=0))
142     {
143       Ayx=(atan2(acc[0],acc[1]))*180/PI;
144     }
145     else
146     {
147       Ayx=t*gyro[2];
148     }*/
149   Ayz=(atan2(acc[1],acc[2]))*180/PI;
150   
151   
152  //kalman filter
153   calculate_xz();
154   calculate_yx();
155   calculate_yz();
156   
157   //sprintf(str, "%d,%d,%d", xz_1, xy_1, x_1);
158   //Serial.print(xz);Serial.print(",");
159   //Serial.print(yx);Serial.print(",");
160   //Serial.print(yz);Serial.print(",");
161   //sprintf(str, "%d,%d,%d,%d,%d,%d", acc[0],acc[1],acc[2],gyro[0],gyro[1],gyro[2]);
162   //sprintf(str, "%d,%d,%d",gyro[0],gyro[1],gyro[2]);
163     Serial.print(Axz);Serial.print(",");
164     //Serial.print(Ayx);Serial.print(",");
165     //Serial.print(Ayz);Serial.print(",");
166   //Serial.print(str);
167   //o=gyro[2];//w=acc[2];
168   //Serial.print(o);Serial.print(",");
169   //Serial.print(w);Serial.print(",");
170   Serial.print("\n");
171 
172   
173   //delay(50);
174 }
175 void calculate_xz()
176 {
177 
178  xz=xz+t*gyro[1];
179  p_xz=p_xz+q_xz;
180  k_xz=p_xz/(p_xz+r_xz);
181  xz=xz+k_xz*(Axz-xz);
182  p_xz=(1-k_xz)*p_xz;
183 }
184 void calculate_yx()
185 {
186   
187   yx=yx+t*gyro[2];
188   p_yx=p_yx+q_yx;
189   k_yx=p_yx/(p_yx+r_yx);
190   yx=yx+k_yx*(Ayx-yx);
191   p_yx=(1-k_yx)*p_yx;
192 
193 }
194 void calculate_yz()
195 {
196   yz=yz+t*gyro[0];
197   p_yz=p_yz+q_yz;
198   k_yz=p_yz/(p_yz+r_yz);
199   yz=yz+k_yz*(Ayz-yz);
200   p_yz=(1-k_yz)*p_yz;
201  
202 }
203 
204 
205 //---------------- Functions
206 //Writes val to address register on ACC
207 void writeTo(int DEVICE, byte address, byte val) {
208    Wire.beginTransmission(DEVICE); //start transmission to ACC 
209    Wire.write(address);        // send register address
210    Wire.write(val);        // send value to write
211    Wire.endTransmission(); //end transmission
212 }
213 
214 
215 //reads num bytes starting from address register on ACC in to buff array
216 void readFrom(int DEVICE, byte address, int num, byte buff[]) {
217   Wire.beginTransmission(DEVICE); //start transmission to ACC 
218   Wire.write(address);        //sends address to read from
219   Wire.endTransmission(); //end transmission
220   
221   Wire.beginTransmission(DEVICE); //start transmission to ACC
222   Wire.requestFrom(DEVICE, num);    // request 6 bytes from ACC
223   
224   int i = 0;
225   while(Wire.available())    //ACC may send less than requested (abnormal)
226   { 
227     buff[i] = Wire.read(); // receive a byte
228     i++;
229   }
230   Wire.endTransmission(); //end transmission
231 }

 

标签:11,FILTER,int,滤波,Value,filter,算法,Filter
From: https://www.cnblogs.com/jiayezi/p/17674853.html

相关文章

  • Java:使用javax.crypto.Cipher的AES算法实现数据加密解密
    AES算法加密Stringalgorithm="AES/ECB/PKCS5Padding";//定义加密算法Stringkey="1234567890123456";//这是待加密的信息Stringmessage="HelloWorld.";//这是待加密的信息Ciphercipher=Cipher.getInstance(algorithm);cipher.init(Cipher.ENCRYPT......
  • 代码随想录算法训练营第二十五天| 216.组合总和III 17.电话号码的字母组合
     216.组合总和III    卡哥建议:如果把 组合问题理解了,本题就容易一些了。    题目链接/文章讲解:https://programmercarl.com/0216.%E7%BB%84%E5%90%88%E6%80%BB%E5%92%8CIII.html   视频讲解:https://www.bilibili.com/video/BV1wg411873x  做题思路:......
  • win11右键显示更多选项怎么关闭?
    win11右键显示更多选项怎么关闭?在新的Win11操作系统之中,微软为了美化界面,将右键菜单进行了整合更改,但是实用性却大幅度下降,引起了很多用户的反感,并迫切希望能够将Win11显示更多选项这个反人类的设置恢复成Win10的状态。如果恰巧您也有相同的需求,那么请您不要错过以下内容。......
  • ORACLE 11.2.0.4 RAC Cluster not starting cssd with Cannot get GPnP profile
       最近,处理一次oracle11.2.0.4raccluster由于cssd无法启动,导致集群一个节点的CRS集群无法正常启动的故障。原本,计划变更是从ASM剔除磁盘,解除存储到数据库服务器的映射;磁盘已经成功从ASM剔除,也已经成功从存储解除到操作系统的映射,为了验证磁盘剔除是否对集群有影响,重启了......
  • 【DBN分类】基于北方苍鹰算法优化深度置信网络NGO-DBN实现轴承故障分类matlab代码
    ✅作者简介:热爱科研的Matlab仿真开发者,修心和技术同步精进,matlab项目合作可私信。......
  • IIR滤波器算法
    IIR(InfiniteImpulseResponse)滤波器是一类递归型数字滤波器,其输出信号不仅与当前的输入信号有关,还与之前的输入和输出信号有关。因此,IIR滤波器的阶数相对较低,可以实现更为复杂的频率响应。IIR滤波器的数学模型描述如下:其中,x(n)和y(n)分别表示输入信号和输出信号,bk和ak分别为前向系......
  • 如何使用C++11原子操作实现自旋锁
    什么是自旋锁?C++自旋锁是一种低层次的同步原语,用于保护共享资源的访问。自旋锁是一种轻量级的锁,适用于短时间的资源锁定。自旋锁的特点:当一个线程尝试获取已经被另一个线程占有的自旋锁时,这个线程会进入一个循环(自旋),在这个循环中它不断地检查锁是否已经被释放。如果锁已经被释放,那......
  • 如何有效的刷算法题
    1.难度循序渐进2.按算法分类选题3.解题三部曲a.看懂题目b.分析、推导题目c.将思路转换成代码 https://github.com/MisterBooo/LeetCodeAnimationhttps://github.com/azl397985856/leetcodehttps://github.com/jwasham/coding-interview-university/blob/ma......
  • centos stream 9 编译 python3.11.5 源代码
    wgethttps://www.python.org/ftp/python/3.11.5/Python-3.11.5.tgztar-zxvfPython-3.11.5.tgzcdPython-3.11.5执行配置帮助./configure--help主要是想设置安装路径Installationdirectories:--prefix=PREFIXinstallarchitecture-independentfilesinP......
  • 假期周进度报告11
    本周(8.27-9.2)主要开展大数据知识的学习和python知识以及调查报告的学习。下周继续学习大数据知识和python知识。周日,进行大数据知识和python知识的学习,决定专心学习大数据和python,学了黑马第五章,遇到的问题去网络查找资料并最终查找到解决方案累积到我的收藏夹中,为下次遇到问题可......