11種濾波算法程序大全(含源代碼)
時間:2021-11-12 14:17:53
手機(jī)看文章
掃描二維碼
隨時隨地手機(jī)看文章
[導(dǎo)讀]1、限幅濾波法(又稱程序判斷濾波法)/*A、名稱:限幅濾波法(又稱程序判斷濾波法)B、方法:??根據(jù)經(jīng)驗判斷,確定兩次采樣允許的最大偏差值(設(shè)為A),??每次檢測到新值時判斷:??如果本次值與上次值之差A(yù),則本次值無效,放棄本次值,用上次值代替本次值。C、優(yōu)點(diǎn):??能有效克服因偶...

1、限幅濾波法(又稱程序判斷濾波法)
/*
A、名稱:限幅濾波法(又稱程序判斷濾波法)
B、方法:
? ? 根據(jù)經(jīng)驗判斷,確定兩次采樣允許的最大偏差值(設(shè)為A),
? ? 每次檢測到新值時判斷:
? ? 如果本次值與上次值之差<=A,則本次值有效,
? ? 如果本次值與上次值之差>A,則本次值無效,放棄本次值,用上次值代替本次值。
C、優(yōu)點(diǎn):
? ? 能有效克服因偶然因素引起的脈沖干擾。
D、缺點(diǎn):
? ? 無法抑制那種周期性的干擾。
? ? 平滑度差。
E、整理:shenhaiyu 2013-11-01
*/
int Filter_Value;
int Value;
void setup() {
??Serial.begin(9600);? ?? ? // 初始化串口通信
??randomSeed(analogRead(0)); // 產(chǎn)生隨機(jī)種子
??Value = 300;
}
void loop() {
??Filter_Value = Filter();? ?? ? // 獲得濾波器輸出值
??Value = Filter_Value;? ?? ?? ? // 最近一次有效采樣的值,該變量為全局變量
??Serial.println(Filter_Value); // 串口輸出
??delay(50);
}
// 用于隨機(jī)產(chǎn)生一個300左右的當(dāng)前值
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、中位值濾波法
/*
A、名稱:中位值濾波法
B、方法:
? ? 連續(xù)采樣N次(N取奇數(shù)),把N次采樣值按大小排列,
? ? 取中間值為本次有效值。
C、優(yōu)點(diǎn):
? ? 能有效克服因偶然因素引起的波動干擾;
? ? 對溫度、液位的變化緩慢的被測參數(shù)有良好的濾波效果。
D、缺點(diǎn):
? ? 對流量、速度等快速變化的參數(shù)不宜。
E、整理:shenhaiyu 2013-11-01
*/
int Filter_Value;
void setup() {
??Serial.begin(9600);? ?? ? // 初始化串口通信
??randomSeed(analogRead(0)); // 產(chǎn)生隨機(jī)種子
}
void loop() {
??Filter_Value = Filter();? ?? ? // 獲得濾波器輸出值
??Serial.println(Filter_Value); // 串口輸出
??delay(50);
}
// 用于隨機(jī)產(chǎn)生一個300左右的當(dāng)前值
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、算術(shù)平均濾波法
/*
A、名稱:算術(shù)平均濾波法
B、方法:
? ? 連續(xù)取N個采樣值進(jìn)行算術(shù)平均運(yùn)算:
? ? N值較大時:信號平滑度較高,但靈敏度較低;
? ? N值較小時:信號平滑度較低,但靈敏度較高;
? ? N值的選取:一般流量,N=12;壓力:N=4。
C、優(yōu)點(diǎn):
? ? 適用于對一般具有隨機(jī)干擾的信號進(jìn)行濾波;
? ? 這種信號的特點(diǎn)是有一個平均值,信號在某一數(shù)值范圍附近上下波動。
D、缺點(diǎn):
? ? 對于測量速度較慢或要求數(shù)據(jù)計算速度較快的實時控制不適用;
? ? 比較浪費(fèi)RAM。
E、整理:shenhaiyu 2013-11-01
*/
int Filter_Value;
void setup() {
??Serial.begin(9600);? ?? ? // 初始化串口通信
??randomSeed(analogRead(0)); // 產(chǎn)生隨機(jī)種子
}
void loop() {
??Filter_Value = Filter();? ?? ? // 獲得濾波器輸出值
??Serial.println(Filter_Value); // 串口輸出
??delay(50);
}
// 用于隨機(jī)產(chǎn)生一個300左右的當(dāng)前值
int Get_AD() {
??return random(295, 305);
}
// 算術(shù)平均濾波法
#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、遞推平均濾波法(又稱滑動平均濾波法)
/*
A、名稱:遞推平均濾波法(又稱滑動平均濾波法)
B、方法:
? ? 把連續(xù)取得的N個采樣值看成一個隊列,隊列的長度固定為N,
? ? 每次采樣到一個新數(shù)據(jù)放入隊尾,并扔掉原來隊首的一次數(shù)據(jù)(先進(jìn)先出原則),
? ? 把隊列中的N個數(shù)據(jù)進(jìn)行算術(shù)平均運(yùn)算,獲得新的濾波結(jié)果。
? ? N值的選?。毫髁?,N=12;壓力,N=4;液面,N=4-12;溫度,N=1-4。
C、優(yōu)點(diǎn):
? ? 對周期性干擾有良好的抑制作用,平滑度高;
? ? 適用于高頻振蕩的系統(tǒng)。
D、缺點(diǎn):
? ? 靈敏度低,對偶然出現(xiàn)的脈沖性干擾的抑制作用較差;
? ? 不易消除由于脈沖干擾所引起的采樣值偏差;
? ? 不適用于脈沖干擾比較嚴(yán)重的場合;
? ? 比較浪費(fèi)RAM。
E、整理:shenhaiyu 2013-11-01
*/
int Filter_Value;
void setup() {
??Serial.begin(9600);? ?? ? // 初始化串口通信
??randomSeed(analogRead(0)); // 產(chǎn)生隨機(jī)種子
}
void loop() {
??Filter_Value = Filter();? ?? ? // 獲得濾波器輸出值
??Serial.println(Filter_Value); // 串口輸出
??delay(50);
}
// 用于隨機(jī)產(chǎn)生一個300左右的當(dāng)前值
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]; // 所有數(shù)據(jù)左移,低位仍掉
? ? filter_sum = filter_buf[i];
??}
??return (int)(filter_sum / FILTER_N);
}
5、中位值平均濾波法(又稱防脈沖干擾平均濾波法)
/*
A、名稱:中位值平均濾波法(又稱防脈沖干擾平均濾波法)
B、方法:
? ? 采一組隊列去掉最大值和最小值后取平均值,
? ? 相當(dāng)于“中位值濾波法” “算術(shù)平均濾波法”。
? ? 連續(xù)采樣N個數(shù)據(jù),去掉一個最大值和一個最小值,
? ? 然后計算N-2個數(shù)據(jù)的算術(shù)平均值。
? ? N值的選取:3-14。
C、優(yōu)點(diǎn):
? ? 融合了“中位值濾波法” “算術(shù)平均濾波法”兩種濾波法的優(yōu)點(diǎn)。
? ? 對于偶然出現(xiàn)的脈沖性干擾,可消除由其所引起的采樣值偏差。
? ? 對周期干擾有良好的抑制作用。
? ? 平滑度高,適于高頻振蕩的系統(tǒng)。
D、缺點(diǎn):
? ? 計算速度較慢,和算術(shù)平均濾波法一樣。
? ? 比較浪費(fèi)RAM。
E、整理:shenhaiyu 2013-11-01
*/
int Filter_Value;
void setup() {
??Serial.begin(9600);? ?? ? // 初始化串口通信
??randomSeed(analogRead(0)); // 產(chǎn)生隨機(jī)種子
}
void loop() {
??Filter_Value = Filter();? ?? ? // 獲得濾波器輸出值
??Serial.println(Filter_Value); // 串口輸出
??delay(50);
}
// 用于隨機(jī)產(chǎn)生一個300左右的當(dāng)前值
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、限幅平均濾波法
/*
A、名稱:限幅平均濾波法
B、方法:
? ? 相當(dāng)于“限幅濾波法” “遞推平均濾波法”;
? ? 每次采樣到的新數(shù)據(jù)先進(jìn)行限幅處理,
? ? 再送入隊列進(jìn)行遞推平均濾波處理。
C、優(yōu)點(diǎn):
? ? 融合了兩種濾波法的優(yōu)點(diǎn);
? ? 對于偶然出現(xiàn)的脈沖性干擾,可消除由于脈沖干擾所引起的采樣值偏差。
D、缺點(diǎn):
? ? 比較浪費(fèi)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)); // 產(chǎn)生隨機(jī)種子
??filter_buf[FILTER_N - 2] = 300;
}
void loop() {
??Filter_Value = Filter();? ?? ? // 獲得濾波器輸出值
??Serial.println(Filter_Value); // 串口輸出
??delay(50);
}
// 用于隨機(jī)產(chǎn)生一個300左右的當(dāng)前值
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、一階滯后濾波法
/*
A、名稱:一階滯后濾波法
B、方法:
? ? 取a=0-1,本次濾波結(jié)果=(1-a)*本次采樣值 a*上次濾波結(jié)果。
C、優(yōu)點(diǎn):
? ? 對周期性干擾具有良好的抑制作用;
? ? 適用于波動頻率較高的場合。
D、缺點(diǎn):
? ? 相位滯后,靈敏度低;
? ? 滯后程度取決于a值大??;
? ? 不能消除濾波頻率高于采樣頻率1/2的干擾信號。
E、整理:shenhaiyu 2013-11-01
*/
int Filter_Value;
int Value;
void setup() {
??Serial.begin(9600);? ?? ? // 初始化串口通信
??randomSeed(analogRead(0)); // 產(chǎn)生隨機(jī)種子
??Value = 300;
}
void loop() {
??Filter_Value = Filter();? ?? ? // 獲得濾波器輸出值
??Serial.println(Filter_Value); // 串口輸出
??delay(50);
}
// 用于隨機(jī)產(chǎn)生一個300左右的當(dāng)前值
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、加權(quán)遞推平均濾波法
/*
A、名稱:加權(quán)遞推平均濾波法
B、方法:
? ? 是對遞推平均濾波法的改進(jìn),即不同時刻的數(shù)據(jù)加以不同的權(quán);
? ? 通常是,越接近現(xiàn)時刻的數(shù)據(jù),權(quán)取得越大。
? ? 給予新采樣值的權(quán)系數(shù)越大,則靈敏度越高,但信號平滑度越低。
C、優(yōu)點(diǎn):
? ? 適用于有較大純滯后時間常數(shù)的對象,和采樣周期較短的系統(tǒng)。
D、缺點(diǎn):
? ? 對于純滯后時間常數(shù)較小、采樣周期較長、變化緩慢的信號;
? ? 不能迅速反應(yīng)系統(tǒng)當(dāng)前所受干擾的嚴(yán)重程度,濾波效果差。
E、整理:shenhaiyu 2013-11-01
*/
int Filter_Value;
void setup() {
??Serial.begin(9600);? ?? ? // 初始化串口通信
??randomSeed(analogRead(0)); // 產(chǎn)生隨機(jī)種子
}
void loop() {
??Filter_Value = Filter();? ?? ? // 獲得濾波器輸出值
??Serial.println(Filter_Value); // 串口輸出
??delay(50);
}
// 用于隨機(jī)產(chǎn)生一個300左右的當(dāng)前值
int Get_AD() {
??return random(295, 305);
}
// 加權(quán)遞推平均濾波法
#define FILTER_N 12
int coe[FILTER_N] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12};? ? // 加權(quán)系數(shù)表
int sum_coe = 1 2 3 4 5 6 7 8 9 10 11 12; // 加權(quán)系數(shù)和
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]; // 所有數(shù)據(jù)左移,低位仍掉
? ? filter_sum = filter_buf[i] * coe[i];
??}
??filter_sum /= sum_coe;
??return filter_sum;
}
9、消抖濾波法
/*
A、名稱:消抖濾波法
B、方法:
? ? 設(shè)置一個濾波計數(shù)器,將每次采樣值與當(dāng)前有效值比較:
? ? 如果采樣值=當(dāng)前有效值,則計數(shù)器清零;
? ? 如果采樣值<>當(dāng)前有效值,則計數(shù)器 1,并判斷計數(shù)器是否>=上限N(溢出);
? ? 如果計數(shù)器溢出,則將本次值替換當(dāng)前有效值,并清計數(shù)器。
C、優(yōu)點(diǎn):
? ? 對于變化緩慢的被測參數(shù)有較好的濾波效果;
? ? 可避免在臨界值附近控制器的反復(fù)開/關(guān)跳動或顯示器上數(shù)值抖動。
D、缺點(diǎn):
? ? 對于快速變化的參數(shù)不宜;
? ? 如果在計數(shù)器溢出的那一次采樣到的值恰好是干擾值,則會將干擾值當(dāng)作有效值導(dǎo)入系統(tǒng)。
E、整理:shenhaiyu 2013-11-01
*/
int Filter_Value;
int Value;
void setup() {
??Serial.begin(9600);? ?? ? // 初始化串口通信
??randomSeed(analogRead(0)); // 產(chǎn)生隨機(jī)種子
??Value = 300;
}
void loop() {
??Filter_Value = Filter();? ?? ? // 獲得濾波器輸出值
??Serial.println(Filter_Value); // 串口輸出
??delay(50);
}
// 用于隨機(jī)產(chǎn)生一個300左右的當(dāng)前值
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、限幅消抖濾波法
/*
A、名稱:限幅消抖濾波法
B、方法:
? ? 相當(dāng)于“限幅濾波法” “消抖濾波法”;
? ? 先限幅,后消抖。
C、優(yōu)點(diǎn):
? ? 繼承了“限幅”和“消抖”的優(yōu)點(diǎn);
? ? 改進(jìn)了“消抖濾波法”中的某些缺陷,避免將干擾值導(dǎo)入系統(tǒng)。
D、缺點(diǎn):
? ? 對于快速變化的參數(shù)不宜。
E、整理:shenhaiyu 2013-11-01
*/
int Filter_Value;
int Value;
void setup() {
??Serial.begin(9600);? ?? ? // 初始化串口通信
??randomSeed(analogRead(0)); // 產(chǎn)生隨機(jī)種子
??Value = 300;
}
void loop() {
??Filter_Value = Filter();? ?? ? // 獲得濾波器輸出值
??Serial.println(Filter_Value); // 串口輸出
??delay(50);
}
// 用于隨機(jī)產(chǎn)生一個300左右的當(dāng)前值
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、卡爾曼濾波(非擴(kuò)展卡爾曼)
#include // 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)