目錄十、限幅消抖濾波法十一、卡爾曼濾波法十、限幅消抖濾波算法A、名稱限幅消抖濾波法B、方法相當(dāng)于“限幅濾波法”“消抖濾波法”先限幅后消抖。C、優(yōu)點繼承了“限幅”和“消抖”的優(yōu)點改進了“消抖濾波法”中的某些缺陷避免將干擾值導(dǎo)入系統(tǒng)。D、缺點對于快速變化的參數(shù)不宜。int Filter_Value; int Value; void setup(void) { Serial.begin(9600); // 初始化串口通信 randomSeed(analogRead(0)); // 產(chǎn)生隨機種子 Value 300; } void loop(void) { Filter_Value Filter(); // 獲得濾波器輸出值 Serial.println(Filter_Value); // 串口輸出 delay(50); } // 用于隨機產(chǎn)生一個300左右的當(dāng)前值 int Get_AD(void) { return random(295, 305); } // 限幅消抖濾波法 #define FILTER_A 1 #define FILTER_N 5 int i 0; int Filter(void) { 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; }十一、卡爾曼濾波算法不是擴展的但是輸出平穩(wěn)的俯仰和滾轉(zhuǎn)應(yīng)該夠了。#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(void) { //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(void) { 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) { 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 xz0,yx0,yz0; float p_xz1,p_yx1,p_yz1; float q_xz0.0025,q_yx0.0025,q_yz0.0025; float k_xz0,k_yx0,k_yz0; float r_xz0.25,r_yx0.25,r_yz0.25; //int acc_temp[3]; //float acc[3]; int acc[3]; int gyro[4]; float Axz; float Ayx; float Ayz; float t0.025; void setup(void) { Serial.begin(9600); Wire.begin(); initAcc(); initGyro(); } //unsigned long timer 0; //float o; void loop(void) { 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]; //rsqrt(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; 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); //ogyro[2];//wacc[2]; //Serial.print(o);Serial.print(,); //Serial.print(w);Serial.print(,); Serial.print(\n); //delay(50); } void calculate_xz(void) { xzxzt*gyro[1]; p_xzp_xzq_xz; k_xzp_xz/(p_xzr_xz); xzxzk_xz*(Axz-xz); p_xz(1-k_xz)*p_xz; } void calculate_yx(void) { yxyxt*gyro[2]; p_yxp_yxq_yx; k_yxp_yx/(p_yxr_yx); yxyxk_yx*(Ayx-yx); p_yx(1-k_yx)*p_yx; } void calculate_yz(void) { yzyzt*gyro[0]; p_yzp_yzq_yz; k_yzp_yz/(p_yzr_yz); yzyzk_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 }人生天地之間若白駒之過隙忽然而已。覺得不錯動動發(fā)財?shù)男∈贮c個贊哦