加速度计与陀螺仪融合一阶互补滤波、二阶互补滤波、卡尔曼滤波核心程序.docx
一阶互补/a=tau/(tau+1.ooptime)UnewAng1.e=ang1.emeasuredwiatan2using(heacce1.erometer加速度传感器输出值UncwRatc=ang1.emeasuredusingthegyro/100ptime=1.ooptimeinni1.1.is()f1.oat1.au=0.075:f1.oata=0.():f1.oatCon1.en>entary(f1.oatnewAng1.e,f1.oatnewRate,int100ptinie)(f1.oat<itC=f1.oat(1.ooptime)/1000.0：a=tau(tau+dtC);下行代码更改成白色，下/后复原成其他旗色即可看到x-ang1.eC=;returnx_ang1.cC;二阶互补UncwAng1.c=ang1.enwasuredwithatan2usingtheaccc1.cromctcrUnewRate=ang1.emeasuredusingthegyro/100ptiine=1.ooptimeinni1.1.is()f1.oatConwntarj'2(11oatncwAng1.c.f1.oatncwRatc.int1.ptime)f1.oatk=10;f1.oatdtc2=11oat(1.oop1.ime)/1(KK),0;x1.=(ncwAng1.c-x_ang1.c2C)*k*k;y1.=dtc2*x1.+y:x2=:此行代码更改成白色，下栽后复原成其他演色即可看到x-ang1.e2C=dic2*x2+x-ang1.e2C;returnx_ang1.c2C:Hcrv1.oowejusthavetoSe1.thCkandmagica1.1.yWCgettheang1.e.卡尔曼滤波/KasBotV1.-Ka1.manfi1.termodu1.ef1.oatf1.oatf1.oatf1.oatx-bis=0;f1.oatP-OO=0.P_01.=0.P_IO=0.P_11=0;f1.oaty.S:11oatK_(),K_1.;/newAng1.e=ang1.emeasuredwithatan2usingtheacce1.erometerUnewRate=ang1.emeasuredusingthegyro/100ptime=1.ooptimeinni1.1.is()f1.oatka1.manCa1.cu1.atc(f1.oatncwAng1.e.f1.oatncwRatc.int1.ptimc)f1.oatdt=11oat(1.ootine)1.(XX);x-ang1.e+=dt*(newRate-x_bias);该卡代码更改成白色，下衣后复原成其他演色即可,到P_00+=;PJ)1+=;P_10+=;P-I1.+=;y=newAng1.e-x_ang1.e:S=P_00+R_ang1.e;K_0=P_00/S;K_1=P_10/S;x-ang1.e+=K_0*y;x_bias+=K_1.*y;P_00-=K_0*P_00:P_OI-=K_0*P_OI;P_IO-=K_1*P_(X);P_1.1.-=K_1.*P_OI;returnx-ang1.e;ITogettheanswer,youhavetoSe1.3parameters:Qjang1.e.R_ang1.e.R_gyn>.具体卡尔曼滤波*-4-indcnt-tabs-nxicf11c-basic-offsctrX:tab-width:8:-vi:setIJi=8: $1.d:tih.c.1200327/0918:23:29johnExpS衾 1dimensiona1.1.i1.tsensorusingdua1.axisaixe1.eroneter andsing1.eaxisangu1.arrategyro.hctwosensorsarcfused viaatwostateKa1.manfi1.ter,withonestatebeingtheang1.e tandheothersta1.ebeingthegyrobias. Gyrobiasisautomatica1.1.ytrackedbythefi1.ter.Thisseems 1.ikemagic.今 P1.easenoteIhaUhekarc1.otsofcomnen1.sin(hefunc1.kmsandoinb1.ocksbeforetheFunctions.Ka1.manfi1.teringisana1.readycomp1.ex subject,madeevenmoresobyextensivehandoptimizationstotheCcode分(hatimp1.ementsthefi1.ter.vetriedtomakeaneffo11ofexp1.aining thep1.irniz4iti0ns.bu1.fee1.freetosendmai1.Iothe11ui1.ing1.ist. autopi1.ot-,withquestionsaboutthiscode.* (c)2003Trarnme1.1.Hudson<>衾 *Thisparioftheautopi1.otOfIbOanIcodepackage. Autapi1.acisfreesoftware;youcanredistributeitndrormodify titUix1.erthetermsoftheGNUGenera1.Pub1.ic1.icenseaspub1.ishedby (heFreeSoftwareFoundation:eitherversion2ofIhc1.icense,or (atyouroption)any1.aterversion., Autopi1.otisdistributedinthehopethatitwi1.1.beusefu1. butWITHOUTANYWARRANTY:withoutevenIhCimp1.iedwarrantyof MERCHANTABI1.ITYorHTNESSEORAPARneU1.ARPURPOSE.Seethe GNUGenera1.Pub1.ic1.icenseformoredetai1.s.*终Ywshou1.dhavereceivedacopyoftheGNUGenera1.Pub1.ic1.icense a1.ongwithAutopi1.ot:ifnot.writetotheFreeSoftware Foundacion,Inc.,59Temp1.eP1.ace,Suite330.Bos1.on,MA021.1.1.-1.307USA#inc1.udc<maih.h>Ourupdaterate.,Ihisishowoftenourstateisupdatedwithgyroratemeasurements.Fornow,WCdoiteverytimean8bitcounterrunningatC1.K/1024exis.Youwi1.1.havetochangeIhisva1.ueityouupdatea1.adifferentra1.e.staticconstf1.oatdt=(1024.0256.0)/KwMK)O().0; Ourcovariancematrix.Thisisupdatedateverytinwstep1.o分determinehowwe1.1.thesensorsaretracking(heactua1.state.Wstaticf1.oatP2112=(,O1.(O,1.s*Ourtwos1.ates,theang1.eandthegybias.Asabyp111.uc1.ofcmpu1.ing* theang1.e,wca1.sohaveanunbiasedangu1.arrateavai1.ab1.e.Thesearc* read-on1.ytotheuserofthemodu1.e.Vf1.oatang1.e:Hoatq_bias:t1.oatrate;* RrepresentsIhCmeasurementcovariancenoise.Inthisease,sei(isaIx1.matrixthatsaysIha1.Weexpect0.3radjitterWfmIhCacix1.erme1.er./staticconstf1.oatRUng1.C=0.3;* Qisa2x2matrixthatrcprcscntstheprocesscovariancenoise.* Inthiscase,itindicateshowmuchWe(rust(heacce1.e)nuer* tre1.ativeIoIhCgy>s./staticconstf1.oatQ.ang1.c=0.001;staticconstf1.oatQ_gyro=0.003;s(a(e_u|xiateisca1.1.edeverydtwithabiasedgyromeasuremenbytheuserofthemodu1.e.I1.updatestheCUrTUn1.ang1.eandrateestimate.JtThepitchgyromeasun>emshou1.dbesca1.edintora1.units,but* doesno1.needanybiasremova1.Thefi1.terwi1.1.Inickthebias.Ourstatevectoris:* X=Iang1.e.gyro_bias* 1(runsthestateestimationfbr½ardviathestatefunctions:分XXdo1.=Iang1.c_do1.gyro_bias_do<* ang1.c-d<H=gyro-gyro_biasjgyro_bias_doi=O* Andupdatesthecovariancematrixviathefunction:.* P(Jo<=A*P+P*A'+Q* AistheJacobianofXdo<withrespecttothestates:* A=d(ang1.e_do(yd(ang1.e)d<ang1.e_do<)/d(gyro_bias)1(d(gyn>_bias_do1.Vd(ang1.e)d(gyro_bias_dot)/d(gyro_bias)* =IO-I1* (OOIyDuctothesma1.1.CPUavai1.ab1.eonthemicrocontro1.1.er,we've* handoptimizedtheCcodeIoon1.ycomputethetermsthatarc食exp1.kidynon-zex),a$we1.1.asexpandedoutIhematrix11th* 1.obedoneinasfewstepsaspossib1.e.Thisde>makeitharder* tor
