1、限制過濾法(又稱程序判斷過濾法)

/* A、名稱：限幅濾波法（又稱程序判斷濾波法） B、方法： 根據(jù)經(jīng)驗(yàn)判斷，確定兩次采樣允許的最大偏差值（設(shè)為A）， 每次檢測(cè)到新值時(shí)判斷： 如果本次值與上次值之差<=A，則本次值有效， 如果本次值與上次值之差>A，則本次值無效，放棄本次值，用上次值代替本次值。 C、優(yōu)點(diǎn)： 能有效克服因偶然因素引起的脈沖干擾。 D、缺點(diǎn)： 無法抑制那種周期性的干擾。 平滑度差。 E、整理：shenhaiyu 2013-11-01 */

intFilter _ ValueintValue

void setup(){ Serial . begin(9600)；//初始化串行通信randomSeed(analog read(0))；//生成隨機(jī)種子值= 300；}

void loop(){ Filter _ Value = Filter；//獲取過濾器輸出值Value = Filter _ Value//最后一次有效采樣的值，全局變量serial . println(Filter _ Value)；//串行輸出延遲(50)；}

//用于隨機(jī)生成一個(gè)300左右的當(dāng)前值intget _ ad () {return random (295，305)；}

//限制篩選法(也叫程序判斷篩選法)# definefilter _ a1intfilter(){ intnewvalue；NewValue = Get _ AD如果(((新值-值)>;FILTER_A) || ((Value - NewValue)>;FILTER_A))返回值；elsereturnNewValue}

2.中值濾波方法

/*A、名稱：中位值濾波法B、方法：連續(xù)采樣N次（N取奇數(shù)），把N次采樣值按大小排列，取中間值為本次有效值。C、優(yōu)點(diǎn)：能有效克服因偶然因素引起的波動(dòng)干擾；對(duì)溫度、液位的變化緩慢的被測(cè)參數(shù)有良好的濾波效果。D、缺點(diǎn)：對(duì)流量、速度等快速變化的參數(shù)不宜。E、整理：shenhaiyu 2013-11-01*/

intFilter _ Value

void setup(){ Serial . begin(9600)；//初始化串行通信randomSeed(analog read(0))；//生成隨機(jī)種子}

void loop(){ Filter _ Value = Filter；//獲取過濾器輸出值serial . println(Filter _ Value)；//串行輸出延遲(50)；}

//用于隨機(jī)生成一個(gè)300左右的當(dāng)前值intget _ ad () {return random (295，305)；}

//中值濾波方法# define filter _ n 101 intfilter(){ int filter _ buf[filter _ n]；inti，j；intfilter _ tempfor(I = 0；i <。FILTER _ N；i++){ filter _ buf[I]= Get _ AD；延遲(1)；}//對(duì)于(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；} } } returnFIlter _ buf[(FILTER _ N-1)/2]；}

3.算術(shù)平均濾波法

/*A、名稱：算術(shù)平均濾波法B、方法：連續(xù)取N個(gè)采樣值進(jìn)行算術(shù)平均運(yùn)算：N值較大時(shí)：信號(hào)平滑度較高，但靈敏度較低；N值較小時(shí)：信號(hào)平滑度較低，但靈敏度較高；N值的選?。阂话懔髁?，N=12；壓力：N=4。C、優(yōu)點(diǎn)：適用于對(duì)一般具有隨機(jī)干擾的信號(hào)進(jìn)行濾波；這種信號(hào)的特點(diǎn)是有一個(gè)平均值，信號(hào)在某一數(shù)值范圍附近上下波動(dòng)。D、缺點(diǎn)：對(duì)于測(cè)量速度較慢或要求數(shù)據(jù)計(jì)算速度較快的實(shí)時(shí)控制不適用；比較浪費(fèi)RAM。E、整理：shenhaiyu 2013-11-01*/

intFilter _ Value

void setup(){ Serial . begin(9600)；//初始化串行通信randomSeed(analog read(0))；//生成隨機(jī)種子}

void loop(){ Filter _ Value = Filter；//獲取過濾器輸出值serial . println(Filter _ Value)；//串行輸出延遲(50)；}

//用于隨機(jī)生成一個(gè)300左右的當(dāng)前值intget _ ad () {return random (295，305)；}

//算術(shù)平均濾波法# define filter _ n12 intfilter(){ inti；int filter _ sum = 0；for(I = 0；i <。FILTER _ N；i++){ filter _ sum+= Get _ AD；延遲(1)；} return(int)(FIlter _ sum/FILTER _ N)；}

4.遞歸平均濾波法(也稱移動(dòng)平均濾波法)

/*A、名稱：遞推平均濾波法（又稱滑動(dòng)平均濾波法）B、方法：把連續(xù)取得的N個(gè)采樣值看成一個(gè)隊(duì)列，隊(duì)列的長(zhǎng)度固定為N，每次采樣到一個(gè)新數(shù)據(jù)放入隊(duì)尾，并扔掉原來隊(duì)首的一次數(shù)據(jù)（先進(jìn)先出原則），把隊(duì)列中的N個(gè)數(shù)據(jù)進(jìn)行算術(shù)平均運(yùn)算，獲得新的濾波結(jié)果。N值的選?。毫髁?，N=12；壓力，N=4；液面，N=4-12；溫度，N=1-4。C、優(yōu)點(diǎn)：對(duì)周期性干擾有良好的抑制作用，平滑度高；適用于高頻振蕩的系統(tǒng)。D、缺點(diǎn)：靈敏度低，對(duì)偶然出現(xiàn)的脈沖性干擾的抑制作用較差；不易消除由于脈沖干擾所引起的采樣值偏差；不適用于脈沖干擾比較嚴(yán)重的場(chǎng)合；比較浪費(fèi)RAM。E、整理：shenhaiyu 2013-11-01*/

intFilter _ Value

void setup(){ Serial . begin(9600)；//初始化串行通信randomSeed(analog read(0))；//生成隨機(jī)種子}

void loop(){ Filter _ Value = Filter；//獲取過濾器輸出值serial . println(Filter _ Value)；//串行輸出延遲(50)；}

//用于隨機(jī)生成一個(gè)300左右的當(dāng)前值intget _ ad () {return random (295，305)；}

//遞歸平均濾波法(也叫移動(dòng)平均濾波法)# define filter _ n ^ 12 int filter _ buf[filter _ n+1]；int filter(){ inti；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、方法：采一組隊(duì)列去掉最大值和最小值后取平均值，相當(dāng)于“中位值濾波法”+“算術(shù)平均濾波法”。連續(xù)采樣N個(gè)數(shù)據(jù)，去掉一個(gè)最大值和一個(gè)最小值，然后計(jì)算N-2個(gè)數(shù)據(jù)的算術(shù)平均值。N值的選取：3-14。C、優(yōu)點(diǎn)：融合了“中位值濾波法”+“算術(shù)平均濾波法”兩種濾波法的優(yōu)點(diǎn)。對(duì)于偶然出現(xiàn)的脈沖性干擾，可消除由其所引起的采樣值偏差。對(duì)周期干擾有良好的抑制作用。平滑度高，適于高頻振蕩的系統(tǒng)。D、缺點(diǎn)：計(jì)算速度較慢，和算術(shù)平均濾波法一樣。比較浪費(fèi)RAM。E、整理：shenhaiyu 2013-11-01*/

intFilter _ Value

void setup(){ Serial . begin(9600)；//初始化串行通信randomSeed(analog read(0))；//生成隨機(jī)種子}

void loop(){ Filter _ Value = Filter；//獲取過濾器輸出值serial . println(Filter _ Value)；//串行輸出延遲(50)；}

//用于隨機(jī)生成一個(gè)300左右的當(dāng)前值intget _ ad () {return random (295，305)；}

//中值平均濾波法(又稱抗脈沖干擾平均濾波法)(算法1)# definefilter _ n 100 int filter(){ inti，j；intfilter_temp，filter _ sum = 0；int FIlter _ buf[FIlter _ N]；for(I = 0；i <。FILTER _ N；i++){ filter _ buf[I]= Get _ AD；延遲(1)；}//對(duì)于(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；} } }//去掉最大和最小極值后，平均值為(I = 1；i <。FILTER _ N-1；i++)filter _ sum+= filter _ buf[I]；returnFIlter _ sum/(FILTER _ N-2)；}

//中值平均濾波法(也叫抗脈沖干擾平均濾波法)(算法二)/* # definefilter _ n100 int filter { int I；int filter _ sum = 0；int filter_max，filter _ minint FIlter _ buf[FIlter _ N]；for(I = 0；i <。FILTER _ N；i++){ filter _ buf[I]= Get _ AD；延遲(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]& lt；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)行限幅處理，再送入隊(duì)列進(jìn)行遞推平均濾波處理。C、優(yōu)點(diǎn)：融合了兩種濾波法的優(yōu)點(diǎn)；對(duì)于偶然出現(xiàn)的脈沖性干擾，可消除由于脈沖干擾所引起的采樣值偏差。D、缺點(diǎn)：比較浪費(fèi)RAM。E、整理：shenhaiyu 2013-11-01*/

# define filter _ N 12 IntFilter _ Value；int FIlter _ buf[FIlter _ N]；

void setup(){ Serial . begin(9600)；//初始化串行通信randomSeed(analog read(0))；//生成隨機(jī)種子FILTER _ buf[FILTER _ N-2]= 300；}

void loop(){ Filter _ Value = Filter；//獲取過濾器輸出值serial . println(Filter _ Value)；//串行輸出延遲(50)；}

//用于隨機(jī)生成一個(gè)300左右的當(dāng)前值intget _ ad () {return random (295，305)；}

//極限平均濾波法# definefilter _ a1intfilter(){ inti；int filter _ sum = 0；FILTER _ buf[FILTER _ N-1]= Get _ AD；if(((FIlter _ buf[FIlter _ N-1]-FIlter _ buf[FIlter _ N-2])> 1；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)：對(duì)周期性干擾具有良好的抑制作用；適用于波動(dòng)頻率較高的場(chǎng)合。D、缺點(diǎn)：相位滯后，靈敏度低；滯后程度取決于a值大??；不能消除濾波頻率高于采樣頻率1/2的干擾信號(hào)。E、整理：shenhaiyu 2013-11-01*/

intFilter _ ValueintValue

void setup(){ Serial . begin(9600)；//初始化串行通信randomSeed(analog read(0))；//生成隨機(jī)種子值= 300；}

void loop(){ Filter _ Value = Filter；//獲取過濾器輸出值serial . println(Filter _ Value)；//串行輸出延遲(50)；}

//用于隨機(jī)生成一個(gè)300左右的當(dāng)前值intget _ ad () {return random (295，305)；}

//一階滯后濾波法# definefilter _ a 0.01 intfilter(){ intnewvalue；NewValue = Get _ ADValue =(int)((float)new Value * FILTER _ A+(1.0-FILTER _ A)*(float)Value)；returnValue}

8.加權(quán)遞歸平均濾波方法

/*A、名稱：加權(quán)遞推平均濾波法B、方法：是對(duì)遞推平均濾波法的改進(jìn)，即不同時(shí)刻的數(shù)據(jù)加以不同的權(quán)；通常是，越接近現(xiàn)時(shí)刻的數(shù)據(jù)，權(quán)取得越大。給予新采樣值的權(quán)系數(shù)越大，則靈敏度越高，但信號(hào)平滑度越低。C、優(yōu)點(diǎn)：適用于有較大純滯后時(shí)間常數(shù)的對(duì)象，和采樣周期較短的系統(tǒng)。D、缺點(diǎn)：對(duì)于純滯后時(shí)間常數(shù)較小、采樣周期較長(zhǎng)、變化緩慢的信號(hào)；不能迅速反應(yīng)系統(tǒng)當(dāng)前所受干擾的嚴(yán)重程度，濾波效果差。E、整理：shenhaiyu 2013-11-01*/

intFilter _ Value

void setup(){ Serial . begin(9600)；//初始化串行通信randomSeed(analog read(0))；//生成隨機(jī)種子}

void loop(){ Filter _ Value = Filter；//獲取過濾器輸出值serial . println(Filter _ Value)；//串行輸出延遲(50)；}

//用于隨機(jī)生成一個(gè)300左右的當(dāng)前值intget _ 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(){ inti；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；returnfilter _ sum}

9.防抖濾波方法

/*A、名稱：消抖濾波法B、方法：設(shè)置一個(gè)濾波計(jì)數(shù)器，將每次采樣值與當(dāng)前有效值比較：如果采樣值=當(dāng)前有效值，則計(jì)數(shù)器清零；如果采樣值<>當(dāng)前有效值，則計(jì)數(shù)器+1，并判斷計(jì)數(shù)器是否>=上限N（溢出）；如果計(jì)數(shù)器溢出，則將本次值替換當(dāng)前有效值，并清計(jì)數(shù)器。C、優(yōu)點(diǎn)：對(duì)于變化緩慢的被測(cè)參數(shù)有較好的濾波效果；可避免在臨界值附近控制器的反復(fù)開/關(guān)跳動(dòng)或顯示器上數(shù)值抖動(dòng)。D、缺點(diǎn)：對(duì)于快速變化的參數(shù)不宜；如果在計(jì)數(shù)器溢出的那一次采樣到的值恰好是干擾值,則會(huì)將干擾值當(dāng)作有效值導(dǎo)入系統(tǒng)。E、整理：shenhaiyu 2013-11-01*/

intFilter _ ValueintValue

void setup(){ Serial . begin(9600)；//初始化串行通信randomSeed(analog read(0))；//生成隨機(jī)種子值= 300；}

void loop(){ Filter _ Value = Filter；//獲取過濾器輸出值serial . println(Filter _ Value)；//串行輸出延遲(50)；}

//用于隨機(jī)生成一個(gè)300左右的當(dāng)前值intget _ ad () {return random (295，305)；}

//去抖動(dòng)濾波法# define filter _ N 12 inti = 0；int FIlter(){ int new _ value；new _ value = Get _ AD如果(值！= new _ value){ i++；if(i >;FILTER _ N){ I = 0；Value = new _ value} } elsei = 0；returnValue}

10、限制抖動(dòng)的濾波方法

/*A、名稱：限幅消抖濾波法B、方法：相當(dāng)于“限幅濾波法”+“消抖濾波法”；先限幅，后消抖。C、優(yōu)點(diǎn)：繼承了“限幅”和“消抖”的優(yōu)點(diǎn)；改進(jìn)了“消抖濾波法”中的某些缺陷，避免將干擾值導(dǎo)入系統(tǒng)。D、缺點(diǎn)：對(duì)于快速變化的參數(shù)不宜。E、整理：shenhaiyu 2013-11-01*/

intFilter _ ValueintValue

void setup(){ Serial . begin(9600)；//初始化串行通信randomSeed(analog read(0))；//生成隨機(jī)種子值= 300；}

void loop(){ Filter _ Value = Filter；//獲取過濾器輸出值serial . println(Filter _ Value)；//串行輸出延遲(50)；}

//用于隨機(jī)生成一個(gè)300左右的當(dāng)前值intget _ ad () {return random (295，305)；}

//削波和抗抖動(dòng)濾波方法# define filter _ a 1 # define filter _ n 5int I = 0；int filter(){ IntNewValue；intnew _ valueNewValue = Get _ AD如果(((新值-值)>;FILTER_A) || ((Value - NewValue)>;FILTER _ A))new _ Value = Value；elsenew _ value = NewValue如果(值！= new _ value){ i++；if(i >;FILTER _ N){ I = 0；Value = new _ value} } elsei = 0；returnValue}

11.卡爾曼濾波(非擴(kuò)展卡爾曼濾波)

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

//加速度計(jì)adxl 345 # define ACC(0x 53)//adxl 345 ACC地址# define a _ TO _ READ(6)//我們每次要讀取的字節(jié)數(shù)(每個(gè)軸兩個(gè)字節(jié))

//陀螺儀ITG3200 #defineGYRO 0x68 //陀螺儀地址，二進(jìn)制= 11101000當(dāng)AD0連接到Vcc時(shí)(參見您的分線板原理圖)# define eg _ SMPLRT _ DIV 0x 15 # define eg _ DLPF _ FS 0x 16 # define eg _ INT _ CFG 0x 17 # define eg _ PWR _ MGM 0x3E

# DEFineg _ TO _ READ 8//x，y，z軸各2字節(jié)

//偏移量是芯片特定的。inta _ offx = 0；inta _ offy = 0；inta _ off z = 0；

int g _ off x = 0；intg _ offy = 0；intg _ off z = 0；////////////////////////

////////////////////////char str[512]；

void initacc {//開啟ADXL345writeTo(ACC，0x2D，0)；writeTo(ACC，0x2D，16)；writeTo(ACC，0x2D，8)；//默認(rèn)情況下，設(shè)備讀數(shù)在+-2g范圍內(nèi)}

voidgetAccelerometerData(int * result){ InteGadress = 0x 32；//adxl 345字節(jié)緩沖器上的第一軸加速度數(shù)據(jù)寄存器[A _ TO _ READ]；讀取自(ACC，regAddress，A_TO_READ，buff)；//從ADXL 345//讀取加速度數(shù)據(jù)，每個(gè)軸讀數(shù)的分辨率為10位，即2字節(jié)。最低有效字節(jié)優(yōu)先??！//因此我們將兩個(gè)字節(jié)都轉(zhuǎn)換成一個(gè)int result[0]=(((int)buff[1])& lt；& lt8)| buff[0]+a _ offx；結(jié)果[1]=(((int)buff[3])& lt；& lt8)| buff[2]+a _ offy；結(jié)果[2]=(((int)buff[5])& lt；& lt8)| buff[4]+a _ off z；}

//初始化陀螺儀無效陀螺儀{/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * ITG 3200 *電源管理設(shè)置為:*時(shí)鐘選擇=內(nèi)部振蕩器*無復(fù)位、無睡眠模式*無待機(jī)模式*采樣速率至= 125赫茲*參數(shù)至+/- 2000度/秒*低通濾波器=5赫茲*無中斷* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */寫至(陀螺儀，G_PWR_MGM，0x 00)；writeTo(GYRO，G_SMPLRT_DIV，0x 07)；// EB，50，80，7F，DE，23，20，F(xiàn)FwriteTo(GYRO，G_DLPF_FS，0x1E)；// +/- 2000 dgrs/sec，1KHz，1E，19writeTo(GYRO，G_INT_CFG，0x 00)；}

voidget陀螺儀數(shù)據(jù)(int * result){/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *陀螺儀ITG-3200 I2cregisters:temp MSB = 1B，temp LSB = 1Cx軸MSB = 1D，x軸LSB = 1Ey軸MSB = 1F，y軸LSB = 20z軸MSB = 21，z軸LSB = 22 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

intregAddress = 0x1Binttemp，x，y，z；字節(jié)緩沖器；readFrom(陀螺儀、regAddress、G_TO_READ、buff)；//從itg 3200 result[0]=((buff[2]& lt；& lt8)| buff[3])+g _ offx；結(jié)果[1]=((buff[4]& lt；& lt8)| buff[5])+g _ offy；結(jié)果[2]=((buff[6]& lt；& lt8)| buff[7])+g _ off z；結(jié)果[3]=(buff[0]& lt；& lt8)| buff[1]；//溫度}

floatxz=0，yx=0，yz = 0；floatp_xz=1，p_yx=1，p _ yz = 1；floatq_xz=0.0025，q_yx=0.0025，q _ yz = 0.0025floatk_xz=0，k_yx=0，k _ yz = 0；floatr_xz=0.25，r_yx=0.25，r _ yz = 0.25//int ACC _ temp[3]；//float ACC[3]；intacc[3]；int gyro[4]；floatAxzfloatAyxflouyz；floatt = 0.025void setup { Serial . begin(9600)；Wire.begininitAccinitGyro}

//無符號(hào)長(zhǎng)定時(shí)器= 0；//float o；void loop { GetAccelerometrdata(ACC)；get陀螺儀數(shù)據(jù)(陀螺儀)；//timer =毫；sprintf(str，" %d，%d，%d，%d，%d，%d "，acc[0]，acc[1]，acc[2]，陀螺儀[0]，陀螺儀[1]，陀螺儀[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])；陀螺[0]=陀螺[0]/14.375；陀螺[1]=陀螺[1]/(-14.375)；陀螺[2]=陀螺[2]/14.375；Axz=(atan2(acc[0]，ACC[2])* 180/PI；Ayx=(atan2(acc[0]，ACC[1])* 180/PI；/*if((acc[0]！= 0)& amp；& amp(acc[1]！=0)){Ayx=(atan2(acc[0]，ACC[1])* 180/PI；} else { Ayx = t *陀螺儀[2]；}*/Ayz=(atan2(acc[1]，ACC[2])* 180/PI；//Kalman filter calculate _ xz；calculate _ yxcalculate _ 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]，陀螺[0]，陀螺[1]，陀螺[2])；//sprintf(str，“%d，%d，%d”，陀螺[0]，陀螺[1]，陀螺[2])；serial . print(Axz)；Serial.print("，")；//serial . print(Ayx)；Serial.print("，")；//serial . print(Ayz)；Serial.print("，")；//Serial . print(str)；//o =陀螺[2]；//w = ACC[2]；//serial . print(o)；Serial.print("，")；//serial . print(w)；Serial.print("，")；serial . print(" n ")；

//延遲(50)；}voidcalculate_xz{

xz = xz+t *陀螺儀[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//將val寫入accvotywriteto(int DEVICe，字節(jié)地址，字節(jié)val)上的地址寄存器{ Wire . Begin Intramission(DEVICe)；//開始傳輸?shù)紸CC Wire.write(地址)；//發(fā)送寄存器address wire . write(val)；//將值發(fā)送到writeWire.endTransmission//結(jié)束傳輸}

//從ACC上的地址寄存器開始讀取字節(jié)數(shù)，以緩沖ArrayVodiReadFrOm(int DEVICe，字節(jié)地址，intnum，字節(jié)緩沖[]){ Wire . Begin Intramission(DEVICe)；//開始傳輸?shù)紸CC Wire.write(地址)；//從Wire.endTransmission發(fā)送要讀取的地址；//end transmission wire . begin in mission(DEVICe)；//開始傳輸?shù)紸CCWire.requestFrom(DEVICE，num)；//從ACCinti請(qǐng)求6字節(jié)= 0；而(Wire.available) //ACC可能發(fā)送的比請(qǐng)求的少(異常){ buff[I]= wire . read；//接收一個(gè)bytei++；} Wire.endTransmission//結(jié)束傳輸}