这个实验无实际应用价值，因为累积误差，哪怕一丁点，过一段时间就误差超级大了，一般在x，y方向的旋转是融合adxl345的数据

#include <Wire.h>
#include <L3G4200D.h>
#include "FilteringScheme.h"
L3G4200D gyro;
KalmanFilter kFilters[3]; //only one value to filter
KalmanFilter kFilterAfter;
MedianMeanFilter meanFilters[3];
unsigned long mTick = 0;
int  mBaseCnt = 100;
float mBaseX = 0.0f;
float mBaseY = 0.0f;
float mBaseZ = 0.0f;float mAngleX = 0;
float mAngleY = 0;
float mAngleZ = 0;
void setup() {Serial.begin(9600);Wire.begin();gyro.enableDefault();mTick = millis();//float qVal = 0.015; //Set Q Kalman Filterfloat rVal = 1.2; //Set K Kalman Filterfloat p = 5.0;float InitVal = 65;kFilters[0].KalmanInit(qVal,rVal,p,InitVal); //Initialize Kalman FilterkFilters[1].KalmanInit(qVal,rVal,p,InitVal);kFilters[2].KalmanInit(qVal,rVal,p,InitVal);kFilterAfter.KalmanInit(qVal,rVal,p,InitVal);//initialBase();
}void loop() {gyro.read();gyro.g.x = kFilters[0].measureRSSI(gyro.g.x);gyro.g.y = kFilters[1].measureRSSI(gyro.g.y);gyro.g.z = kFilters[2].measureRSSI(gyro.g.z);//gyro.g.x = meanFilters[0].Mean(gyro.g.x);if( mBaseCnt > 0){mBaseCnt --;mBaseX += gyro.g.x;mBaseY += gyro.g.y;mBaseZ += gyro.g.z;mBaseX /= 2;mBaseY /= 2;mBaseZ /= 2;}else{gyro.g.x -= mBaseX;gyro.g.y -= mBaseY;gyro.g.z -= mBaseZ;//gyro.g.x = kFilterAfter.measureRSSI(gyro.g.x);float nowTick = millis();float dt = nowTick - mTick;mTick = nowTick;mAngleX = mAngleX + gyro.g.x * dt / 1000.0f * 0.00875;//0.07f;Serial.print("Angle:");Serial.println(mAngleX);}Serial.print("G ");Serial.print("X: ");Serial.print((int)gyro.g.x);Serial.print(" Y: ");Serial.print((int)gyro.g.y);Serial.print(" Z: ");Serial.println((int)gyro.g.z);delay(100);
}