标签归档:算法

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

最近用Arduino做电子秤,为了解决数据的跳变研究了不少滤波算法。网上能找到大把的十大滤波算法帖子,每一篇都不太一样,都号称精编啊,除错啊什么的,可是放到板子里却没一个能正常跑起来的。于是决定自己整理一下这些程序,完美移植到Arduino中。

所以大家看到这个帖子的时候,不要怀疑我重复发帖。我的代码都是经过反复试验,复制到Arduino中就能开跑的成品代码,移植到自己的程序中非常方便。而且都仔细研究了各个算法,把错误都修正了的(别的程序连冒泡算法都是溢出的,不信自己找来细看看),所以也算个小原创吧,在别人基础上的原创。

转载请注明出处:极客工坊  http://www.geek-workshop.com/thread-7694-1-1.html

By shenhaiyu 2013-11-01

 

1、限幅滤波法(又称程序判断滤波法)
2、中位值滤波法
3、算术平均滤波法
4、递推平均滤波法(又称滑动平均滤波法)
5、中位值平均滤波法(又称防脉冲干扰平均滤波法)
6、限幅平均滤波法
7、一阶滞后滤波法
8、加权递推平均滤波法
9、消抖滤波法
10、限幅消抖滤波法
11、新增加 卡尔曼滤波(非扩展卡尔曼),感谢zhangzhe0617分享

程序默认对int类型数据进行滤波,如需要对其他类型进行滤波,只需要把程序中所有int替换成long、float或者double即可。

1、限幅滤波法(又称程序判断滤波法)

ARDUINO 代码
/*
A、名称:限幅滤波法(又称程序判断滤波法)
B、方法:
    根据经验判断,确定两次采样允许的最大偏差值(设为A),
    每次检测到新值时判断:
    如果本次值与上次值之差<=A,则本次值有效,
    如果本次值与上次值之差>A,则本次值无效,放弃本次值,用上次值代替本次值。
C、优点:
    能有效克服因偶然因素引起的脉冲干扰。
D、缺点:
    无法抑制那种周期性的干扰。
    平滑度差。
E、整理:shenhaiyu 2013-11-01
*/
 
int Filter_Value;
int Value;
 
void setup() {
  Serial.begin(9600);       // 初始化串口通信
  randomSeed(analogRead(0)); // 产生随机种子
  Value = 300;
}
 
void loop() {
  Filter_Value = Filter();       // 获得滤波器输出值
  Value = Filter_Value;          // 最近一次有效采样的值,该变量为全局变量
  Serial.println(Filter_Value); // 串口输出
  delay(50);
}
 
// 用于随机产生一个300左右的当前值
int Get_AD() {
  return random(295, 305);
}
 
// 限幅滤波法(又称程序判断滤波法)
#define FILTER_A 1
int Filter() {
  int NewValue;
  NewValue = Get_AD();
  if(((NewValue - Value) > FILTER_A) || ((Value - NewValue) > FILTER_A))
    return Value;
  else
    return NewValue;
}

2、中位值滤波法

ARDUINO 代码
/*
A、名称:中位值滤波法
B、方法:
    连续采样N次(N取奇数),把N次采样值按大小排列,
    取中间值为本次有效值。
C、优点:
    能有效克服因偶然因素引起的波动干扰;
    对温度、液位的变化缓慢的被测参数有良好的滤波效果。
D、缺点:
    对流量、速度等快速变化的参数不宜。
E、整理:shenhaiyu 2013-11-01
*/
 
int Filter_Value;
 
void setup() {
  Serial.begin(9600);       // 初始化串口通信
  randomSeed(analogRead(0)); // 产生随机种子
}
 
void loop() {
  Filter_Value = Filter();       // 获得滤波器输出值
  Serial.println(Filter_Value); // 串口输出
  delay(50);
}
 
// 用于随机产生一个300左右的当前值
int Get_AD() {
  return random(295, 305);
}
 
// 中位值滤波法
#define FILTER_N 101
int 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;
      }
    }
  }
  return filter_buf[(FILTER_N - 1) / 2];
}

3、算术平均滤波法

ARDUINO 代码
/*
A、名称:算术平均滤波法
B、方法:
    连续取N个采样值进行算术平均运算:
    N值较大时:信号平滑度较高,但灵敏度较低;
    N值较小时:信号平滑度较低,但灵敏度较高;
    N值的选取:一般流量,N=12;压力:N=4。
C、优点:
    适用于对一般具有随机干扰的信号进行滤波;
    这种信号的特点是有一个平均值,信号在某一数值范围附近上下波动。
D、缺点:
    对于测量速度较慢或要求数据计算速度较快的实时控制不适用;
    比较浪费RAM。
E、整理:shenhaiyu 2013-11-01
*/
 
int Filter_Value;
 
void setup() {
  Serial.begin(9600);       // 初始化串口通信
  randomSeed(analogRead(0)); // 产生随机种子
}
 
void loop() {
  Filter_Value = Filter();       // 获得滤波器输出值
  Serial.println(Filter_Value); // 串口输出
  delay(50);
}
 
// 用于随机产生一个300左右的当前值
int Get_AD() {
  return random(295, 305);
}
 
// 算术平均滤波法
#define 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、递推平均滤波法(又称滑动平均滤波法)

ARDUINO 代码
/*
A、名称:递推平均滤波法(又称滑动平均滤波法)
B、方法:
    把连续取得的N个采样值看成一个队列,队列的长度固定为N,
    每次采样到一个新数据放入队尾,并扔掉原来队首的一次数据(先进先出原则),
    把队列中的N个数据进行算术平均运算,获得新的滤波结果。
    N值的选取:流量,N=12;压力,N=4;液面,N=4-12;温度,N=1-4。
C、优点:
    对周期性干扰有良好的抑制作用,平滑度高;
    适用于高频振荡的系统。
D、缺点:
    灵敏度低,对偶然出现的脉冲性干扰的抑制作用较差;
    不易消除由于脉冲干扰所引起的采样值偏差;
    不适用于脉冲干扰比较严重的场合;
    比较浪费RAM。
E、整理:shenhaiyu 2013-11-01
*/
 
int Filter_Value;
 
void setup() {
  Serial.begin(9600);       // 初始化串口通信
  randomSeed(analogRead(0)); // 产生随机种子
}
 
void loop() {
  Filter_Value = Filter();       // 获得滤波器输出值
  Serial.println(Filter_Value); // 串口输出
  delay(50);
}
 
// 用于随机产生一个300左右的当前值
int Get_AD() {
  return random(295, 305);
}
 
// 递推平均滤波法(又称滑动平均滤波法)
#define 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、中位值平均滤波法(又称防脉冲干扰平均滤波法)

ARDUINO 代码
/*
A、名称:中位值平均滤波法(又称防脉冲干扰平均滤波法)
B、方法:
    采一组队列去掉最大值和最小值后取平均值,
    相当于“中位值滤波法”+“算术平均滤波法”。
    连续采样N个数据,去掉一个最大值和一个最小值,
    然后计算N-2个数据的算术平均值。
    N值的选取:3-14。
C、优点:
    融合了“中位值滤波法”+“算术平均滤波法”两种滤波法的优点。
    对于偶然出现的脉冲性干扰,可消除由其所引起的采样值偏差。
    对周期干扰有良好的抑制作用。
    平滑度高,适于高频振荡的系统。
D、缺点:
    计算速度较慢,和算术平均滤波法一样。
    比较浪费RAM。
E、整理:shenhaiyu 2013-11-01
*/
 
int Filter_Value;
 
void setup() {
  Serial.begin(9600);       // 初始化串口通信
  randomSeed(analogRead(0)); // 产生随机种子
}
 
void loop() {
  Filter_Value = Filter();       // 获得滤波器输出值
  Serial.println(Filter_Value); // 串口输出
  delay(50);
}
 
// 用于随机产生一个300左右的当前值
int Get_AD() {
  return random(295, 305);
}
 
// 中位值平均滤波法(又称防脉冲干扰平均滤波法)(算法1)
#define FILTER_N 100
int 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];
  return filter_sum / (FILTER_N - 2);
}
 
 
//  中位值平均滤波法(又称防脉冲干扰平均滤波法)(算法2)
/*
#define 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、限幅平均滤波法

ARDUINO 代码
/*
A、名称:限幅平均滤波法
B、方法:
    相当于“限幅滤波法”+“递推平均滤波法”;
    每次采样到的新数据先进行限幅处理,
    再送入队列进行递推平均滤波处理。
C、优点:
    融合了两种滤波法的优点;
    对于偶然出现的脉冲性干扰,可消除由于脉冲干扰所引起的采样值偏差。
D、缺点:
    比较浪费RAM。
E、整理:shenhaiyu 2013-11-01
*/
 
#define 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 loop() {
  Filter_Value = Filter();       // 获得滤波器输出值
  Serial.println(Filter_Value); // 串口输出
  delay(50);
}
 
// 用于随机产生一个300左右的当前值
int Get_AD() {
  return random(295, 305);
}
 
// 限幅平均滤波法
#define 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、一阶滞后滤波法

ARDUINO 代码
/*
A、名称:一阶滞后滤波法
B、方法:
    取a=0-1,本次滤波结果=(1-a)*本次采样值+a*上次滤波结果。
C、优点:
    对周期性干扰具有良好的抑制作用;
    适用于波动频率较高的场合。
D、缺点:
    相位滞后,灵敏度低;
    滞后程度取决于a值大小;
    不能消除滤波频率高于采样频率1/2的干扰信号。
E、整理:shenhaiyu 2013-11-01
*/
 
int Filter_Value;
int Value;
 
void setup() {
  Serial.begin(9600);       // 初始化串口通信
  randomSeed(analogRead(0)); // 产生随机种子
  Value = 300;
}
 
void loop() {
  Filter_Value = Filter();       // 获得滤波器输出值
  Serial.println(Filter_Value); // 串口输出
  delay(50);
}
 
// 用于随机产生一个300左右的当前值
int Get_AD() {
  return random(295, 305);
}
 
// 一阶滞后滤波法
#define FILTER_A 0.01
int Filter() {
  int NewValue;
  NewValue = Get_AD();
  Value = (int)((float)NewValue * FILTER_A + (1.0 - FILTER_A) * (float)Value);
  return Value;
}

8、加权递推平均滤波法

ARDUINO 代码
/*
A、名称:加权递推平均滤波法
B、方法:
    是对递推平均滤波法的改进,即不同时刻的数据加以不同的权;
    通常是,越接近现时刻的数据,权取得越大。
    给予新采样值的权系数越大,则灵敏度越高,但信号平滑度越低。
C、优点:
    适用于有较大纯滞后时间常数的对象,和采样周期较短的系统。
D、缺点:
    对于纯滞后时间常数较小、采样周期较长、变化缓慢的信号;
    不能迅速反应系统当前所受干扰的严重程度,滤波效果差。
E、整理:shenhaiyu 2013-11-01
*/
 
int Filter_Value;
 
void setup() {
  Serial.begin(9600);       // 初始化串口通信
  randomSeed(analogRead(0)); // 产生随机种子
}
 
void loop() {
  Filter_Value = Filter();       // 获得滤波器输出值
  Serial.println(Filter_Value); // 串口输出
  delay(50);
}
 
// 用于随机产生一个300左右的当前值
int Get_AD() {
  return random(295, 305);
}
 
// 加权递推平均滤波法
#define 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;
}

9、消抖滤波法

ARDUINO 代码
/*
A、名称:消抖滤波法
B、方法:
    设置一个滤波计数器,将每次采样值与当前有效值比较:
    如果采样值=当前有效值,则计数器清零;
    如果采样值<>当前有效值,则计数器+1,并判断计数器是否>=上限N(溢出);
    如果计数器溢出,则将本次值替换当前有效值,并清计数器。
C、优点:
    对于变化缓慢的被测参数有较好的滤波效果;
    可避免在临界值附近控制器的反复开/关跳动或显示器上数值抖动。
D、缺点:
    对于快速变化的参数不宜;
    如果在计数器溢出的那一次采样到的值恰好是干扰值,则会将干扰值当作有效值导入系统。
E、整理:shenhaiyu 2013-11-01
*/
 
int Filter_Value;
int Value;
 
void setup() {
  Serial.begin(9600);       // 初始化串口通信
  randomSeed(analogRead(0)); // 产生随机种子
  Value = 300;
}
 
void loop() {
  Filter_Value = Filter();       // 获得滤波器输出值
  Serial.println(Filter_Value); // 串口输出
  delay(50);
}
 
// 用于随机产生一个300左右的当前值
int Get_AD() {
  return random(295, 305);
}
 
// 消抖滤波法
#define FILTER_N 12
int 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;
    }
  }
  else
    i = 0;
  return Value;
}

10、限幅消抖滤波法

ARDUINO 代码
/*
A、名称:限幅消抖滤波法
B、方法:
    相当于“限幅滤波法”+“消抖滤波法”;
    先限幅,后消抖。
C、优点:
    继承了“限幅”和“消抖”的优点;
    改进了“消抖滤波法”中的某些缺陷,避免将干扰值导入系统。
D、缺点:
    对于快速变化的参数不宜。
E、整理:shenhaiyu 2013-11-01
*/
 
int Filter_Value;
int Value;
 
void setup() {
  Serial.begin(9600);       // 初始化串口通信
  randomSeed(analogRead(0)); // 产生随机种子
  Value = 300;
}
 
void loop() {
  Filter_Value = Filter();       // 获得滤波器输出值
  Serial.println(Filter_Value); // 串口输出
  delay(50);
}
 
// 用于随机产生一个300左右的当前值
int Get_AD() {
  return random(295, 305);
}
 
// 限幅消抖滤波法
#define FILTER_A 1
#define FILTER_N 5
int i = 0;
int Filter() {
  int NewValue;
  int new_value;
  NewValue = Get_AD();
  if(((NewValue - Value) > FILTER_A) || ((Value - NewValue) > FILTER_A))
    new_value = Value;
  else
    new_value = NewValue;
  if(Value != new_value) {
    i++;
    if(i > FILTER_N) {
      i = 0;
      Value = new_value;
    }
  }
  else
    i = 0;
  return Value;
}

 

11、卡尔曼滤波(非扩展卡尔曼)

ARDUINO 代码
#include <Wire.h> // I2C library, gyroscope

// Accelerometer ADXL345
#define ACC (0x53)    //ADXL345 ACC address
#define A_TO_READ (6)        //num of bytes we are going to read each time (two bytes for each axis)


// Gyroscope ITG3200 
#define GYRO 0x68 // gyro address, binary = 11101000 when AD0 is connected to Vcc (see schematics of your breakout board)
#define G_SMPLRT_DIV 0x15   
#define G_DLPF_FS 0x16   
#define G_INT_CFG 0x17
#define G_PWR_MGM 0x3E

#define G_TO_READ 8 // 2 bytes for each axis x, y, z


// offsets are chip specific. 
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() {
  //Turning on the ADXL345
  writeTo(ACC, 0x2D, 0);      
  writeTo(ACC, 0x2D, 16);
  writeTo(ACC, 0x2D, 8);
  //by default the device is in +-2g range reading
}

void getAccelerometerData(int* result) {
  int regAddress = 0x32;    //first axis-acceleration-data register on the ADXL345
  byte buff[A_TO_READ];
  
  readFrom(ACC, regAddress, A_TO_READ, buff); //read the acceleration data from the ADXL345
  
  //each axis reading comes in 10 bit resolution, ie 2 bytes.  Least Significat Byte first!!
  //thus we are converting both bytes in to one int
  result[0] = (((int)buff[1]) << 8) | buff[0] + a_offx;   
  result[1] = (((int)buff[3]) << 8) | buff[2] + a_offy;
  result[2] = (((int)buff[5]) << 8) | buff[4] + a_offz;
}

//initializes the gyroscope
void initGyro()
{
  /*****************************************
  * ITG 3200
  * power management set to:
  * clock select = internal oscillator
  *     no reset, no sleep mode
  *   no standby mode
  * sample rate to = 125Hz
  * parameter to +/- 2000 degrees/sec
  * low pass filter = 5Hz
  * no interrupt
  ******************************************/
  writeTo(GYRO, G_PWR_MGM, 0x00);
  writeTo(GYRO, G_SMPLRT_DIV, 0x07); // EB, 50, 80, 7F, DE, 23, 20, FF
  writeTo(GYRO, G_DLPF_FS, 0x1E); // +/- 2000 dgrs/sec, 1KHz, 1E, 19
  writeTo(GYRO, G_INT_CFG, 0x00);
}


void getGyroscopeData(int * result)
{
  /**************************************
  Gyro ITG-3200 I2C
  registers:
  temp MSB = 1B, temp LSB = 1C
  x axis MSB = 1D, x axis LSB = 1E
  y axis MSB = 1F, y axis LSB = 20
  z axis MSB = 21, z axis LSB = 22
  *************************************/

  int regAddress = 0x1B;
  int temp, x, y, z;
  byte buff[G_TO_READ];
  
  readFrom(GYRO, regAddress, G_TO_READ, buff); //read the gyro data from the ITG3200
  
  result[0] = ((buff[2] << 8) | buff[3]) + g_offx;
  result[1] = ((buff[4] << 8) | buff[5]) + g_offy;
  result[2] = ((buff[6] << 8) | buff[7]) + g_offz;
  result[3] = (buff[0] << 8) | buff[1]; // temperature
  
}


float xz=0,yx=0,yz=0;
float p_xz=1,p_yx=1,p_yz=1;
float q_xz=0.0025,q_yx=0.0025,q_yz=0.0025;
float 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;
  float t=0.025;
void setup()
{
  Serial.begin(9600);
  Wire.begin();
  initAcc();
  initGyro();
  
}

//unsigned long timer = 0;
//float o;
void loop()
{
  
  getAccelerometerData(acc);
  getGyroscopeData(gyro);
  //timer = millis();
  sprintf(str, "%d,%d,%d,%d,%d,%d", acc[0],acc[1],acc[2],gyro[0],gyro[1],gyro[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]);
  gyro[0]=gyro[0]/ 14.375;
  gyro[1]=gyro[1]/ (-14.375);
  gyro[2]=gyro[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*gyro[2];
    }*/
  Ayz=(atan2(acc[1],acc[2]))*180/PI;
  
  
//kalman filter
  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],gyro[0],gyro[1],gyro[2]);
  //sprintf(str, "%d,%d,%d",gyro[0],gyro[1],gyro[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");

  
  //delay(50);
}
void calculate_xz()
{

xz=xz+t*gyro[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;
}
void calculate_yx()
{
  
  yx=yx+t*gyro[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;

}
void calculate_yz()
{
  yz=yz+t*gyro[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;

}


//---------------- Functions
//Writes val to address register on ACC
void writeTo(int DEVICE, byte address, byte val) {
   Wire.beginTransmission(DEVICE); //start transmission to ACC 
   Wire.write(address);        // send register address
   Wire.write(val);        // send value to write
   Wire.endTransmission(); //end transmission
}


//reads num bytes starting from address register on ACC in to buff array
void readFrom(int DEVICE, byte address, int num, byte buff[]) {
  Wire.beginTransmission(DEVICE); //start transmission to ACC 
  Wire.write(address);        //sends address to read from
  Wire.endTransmission(); //end transmission
  
  Wire.beginTransmission(DEVICE); //start transmission to ACC
  Wire.requestFrom(DEVICE, num);    // request 6 bytes from ACC
  
  int i = 0;
  while(Wire.available())    //ACC may send less than requested (abnormal)
  { 
    buff[i] = Wire.read(); // receive a byte
    i++;
  }
  Wire.endTransmission(); //end transmission
}

 

本文转自:http://www.geek-workshop.com/thread-7694-1-1.html

 

AC自动机算法详解


    首先简要介绍一下AC自动机:Aho-Corasick automation,该算法在1975年产生于贝尔实验室,是著名的多模匹配算法之一。一个常见的例子就是给出n个单词,再给出一段包含m个字符的文章,让你找出有多少个单词在文章里出现过。要搞懂AC自动机,先得有模式树(字典树)Trie和KMP模式匹配算法的基础知识。AC自动机算法分为3步:构造一棵Trie树,构造失败指针和模式匹配过程。
如果你对KMP算法和了解的话,应该知道KMP算法中的next函数(shift函数或者fail函数)是干什么用的。KMP中我们用两个指针i和j分别表示,A[i-j+ 1..i]与B[1..j]完全相等。也就是说,i是不断增加的,随着i的增加j相应地变化,且j满足以A[i]结尾的长度为j的字符串正好匹配B串的前 j个字符,当A[i+1]≠B[j+1],KMP的策略是调整j的位置(减小j值)使得A[i-j+1..i]与B[1..j]保持匹配且新的B[j+1]恰好与A[i+1]匹配,而next函数恰恰记录了这个j应该调整到的位置。同样AC自动机的失败指针具有同样的功能,也就是说当我们的模式串在Tire上进行匹配时,如果与当前节点的关键字不能继续匹配的时候,就应该去当前节点的失败指针所指向的节点继续进行匹配。
看下面这个例子:给定5个单词:say she shr he her,然后给定一个字符串yasherhs。问一共有多少单词在这个字符串中出现过。我们先规定一下AC自动机所需要的一些数据结构,方便接下去的编程。

[code lang=”c”]
const int kind = 26;
struct node{
node *fail;       //失败指针
node *next[kind]; //Tire每个节点的个子节点(最多个字母)
int count;        //是否为该单词的最后一个节点
node(){           //构造函数初始化
fail=NULL;
count=0;
memset(next,NULL,sizeof(next));
}
}*q[500001];          //队列,方便用于bfs构造失败指针
char keyword[51];     //输入的单词
char str[1000001];    //模式串
int head,tail;        //队列的头尾指针
[/code]

有了这些数据结构之后,就可以开始编程了:
首先,将这5个单词构造成一棵Tire,如图-1所示。

[code lang=”c”]
void insert(char *str,node *root){
node *p=root;
int i=0,index;
while(str[i]){
index=str[i]-‘a’;
if(p->next[index]==NULL) p->next[index]=new node();
p=p->next[index];
i++;
}
p->count++;     //在单词的最后一个节点count+1,代表一个单词
}
[/code]

在构造完这棵Tire之后,接下去的工作就是构造下失败指针。构造失败指针的过程概括起来就一句话:设这个节点上的字母为C,沿着他父亲的失败指针走,直到走到一个节点,他的儿子中也有字母为C的节点。然后把当前节点的失败指针指向那个字母也为C的儿子。如果一直走到了root都没找到,那就把失败指针指向root。具体操作起来只需要:先把root加入队列(root的失败指针指向自己或者NULL),这以后我们每处理一个点,就把它的所有儿子加入队列,队列为空。

[code lang=”c”]
void build_ac_automation(node *root){
int i;
root->fail=NULL;
q[head++]=root;
while(head!=tail){
node *temp=q[tail++];
node *p=NULL;
for(i=0;i<26;i++){
if(temp->next[i]!=NULL){
if(temp==root) temp->next[i]->fail=root;
else{
p=temp->fail;
while(p!=NULL){
if(p->next[i]!=NULL){
temp->next[i]->fail=p->next[i];
break;
}
p=p->fail;
}
if(p==NULL) temp->next[i]->fail=root;
}
q[head++]=temp->next[i];
}
}
}
}
[/code]

从代码观察下构造失败指针的流程:对照图-2来看,首先root的fail指针指向NULL,然后root入队,进入循环。第1次循环的时候,我们需要处理2个节点:root->next[‘h’-‘a’](节点h) 和 root->next[‘s’-‘a’](节点s)。把这2个节点的失败指针指向root,并且先后进入队列,失败指针的指向对应图-2中的(1),(2)两条虚线;第2次进入循环后,从队列中先弹出h,接下来p指向h节点的fail指针指向的节点,也就是root;进入第13行的循环后,p=p->fail也就是p=NULL,这时退出循环,并把节点e的fail指针指向root,对应图-2中的(3),然后节点e进入队列;第3次循环时,弹出的第一个节点a的操作与上一步操作的节点e相同,把a的fail指针指向root,对应图-2中的(4),并入队;第4次进入循环时,弹出节点h(图中左边那个),这时操作略有不同。在程序运行到14行时,由于p->next[i]!=NULL(root有h这个儿子节点,图中右边那个),这样便把左边那个h节点的失败指针指向右边那个root的儿子节点h,对应图-2中的(5),然后h入队。以此类推:在循环结束后,所有的失败指针就是图-2中的这种形式。

最后,我们便可以在AC自动机上查找模式串中出现过哪些单词了。匹配过程分两种情况:(1)当前字符匹配,表示从当前节点沿着树边有一条路径可以到达目标字符,此时只需沿该路径走向下一个节点继续匹配即可,目标字符串指针移向下个字符继续匹配;(2)当前字符不匹配,则去当前节点失败指针所指向的字符继续匹配,匹配过程随着指针指向root结束。重复这2个过程中的任意一个,直到模式串走到结尾为止。

[code lang=”c”]
int query(node *root){
int i=0,cnt=0,index,len=strlen(str);
node *p=root;
while(str[i]){
index=str[i]-‘a’;
while(p->next[index]==NULL && p!=root) p=p->fail;
p=p->next[index];
p=(p==NULL)?root:p;
node *temp=p;
while(temp!=root && temp->count!=-1){
cnt+=temp->count;
temp->count=-1;
temp=temp->fail;
}
i++;
}
return cnt;
}
[/code]

    对照图-2,看一下模式匹配这个详细的流程,其中模式串为yasherhs。对于i=0,1。Trie中没有对应的路径,故不做任何操作;i=2,3,4时,指针p走到左下节点e。因为节点e的count信息为1,所以cnt+1,并且讲节点e的count值设置为-1,表示改单词已经出现过了,防止重复计数,最后temp指向e节点的失败指针所指向的节点继续查找,以此类推,最后temp指向root,退出while循环,这个过程中count增加了2。表示找到了2个单词she和he。当i=5时,程序进入第5行,p指向其失败指针的节点,也就是右边那个e节点,随后在第6行指向r节点,r节点的count值为1,从而count+1,循环直到temp指向root为止。最后i=6,7时,找不到任何匹配,匹配过程结束。

到此为止AC自动机算法的详细过程已经全部介绍结束,看一道例题:http://acm.hdu.edu.cn/showproblem.php?pid=2222

Problem Description

In the modern time, Search engine came into the life of everybody like Google, Baidu, etc.
Wiskey also wants to bring this feature to his image retrieval system.
Every image have a long description, when users type some keywords to find the image, the system will match the keywords with description of image and show the image which the most keywords be matched.
To simplify the problem, giving you a description of image, and some keywords, you should tell me how many keywords will be match.

Input

First line will contain one integer means how many cases will follow by.
Each case will contain two integers N means the number of keywords and N keywords follow. (N <= 10000)
Each keyword will only contains characters ‘a’-‘z’, and the length will be not longer than 50.
The last line is the description, and the length will be not longer than 1000000.

Output

Print how many keywords are contained in the description.

Sample Input

1

5

she

he

say

shr

her

yasherhs

Sample Output

3

[code lang=”c”]
#include <iostream>
using namespace std;

const int kind = 26;
struct node{
node *fail;       //失败指针
node *next[kind]; //Tire每个节点的26个子节点(最多26个字母)
int count;        //是否为该单词的最后一个节点
node(){           //构造函数初始化
fail=NULL;
count=0;
memset(next,NULL,sizeof(next));
}
}*q[500001];          //队列,方便用于bfs构造失败指针
char keyword[51];     //输入的单词
char str[1000001];    //模式串
int head,tail;        //队列的头尾指针

void insert(char *str,node *root){
node *p=root;
int i=0,index;
while(str[i]){
index=str[i]-‘a’;
if(p->next[index]==NULL) p->next[index]=new node();
p=p->next[index];
i++;
}
p->count++;
}
void build_ac_automation(node *root){
int i;
root->fail=NULL;
q[head++]=root;
while(head!=tail){
node *temp=q[tail++];
node *p=NULL;
for(i=0;i<26;i++){
if(temp->next[i]!=NULL){
if(temp==root) temp->next[i]->fail=root;
else{
p=temp->fail;
while(p!=NULL){
if(p->next[i]!=NULL){
temp->next[i]->fail=p->next[i];
break;
}
p=p->fail;
}
if(p==NULL) temp->next[i]->fail=root;
}
q[head++]=temp->next[i];
}
}
}
}
int query(node *root){
int i=0,cnt=0,index,len=strlen(str);
node *p=root;
while(str[i]){
index=str[i]-‘a’;
while(p->next[index]==NULL && p!=root) p=p->fail;
p=p->next[index];
p=(p==NULL)?root:p;
node *temp=p;
while(temp!=root && temp->count!=-1){
cnt+=temp->count;
temp->count=-1;
temp=temp->fail;
}
i++;
}
return cnt;
}
int main(){
int n,t;
scanf("%d",&t);
while(t–){
head=tail=0;
node *root=new node();
scanf("%d",&n);
getchar();
while(n–){
gets(keyword);
insert(keyword,root);
}
build_ac_automation(root);
scanf("%s",str);
printf("%d\n",query(root));
}
return 0;
}
[/code]

PS:原创,转载请注明出处

本文转自:AC自动机算法详解