加速度计与陀螺仪融合一阶互补滤波、二阶互补滤波、卡尔曼滤波核心程序.docx

《加速度计与陀螺仪融合一阶互补滤波、二阶互补滤波、卡尔曼滤波核心程序.docx》由会员分享，可在线阅读，更多相关《加速度计与陀螺仪融合一阶互补滤波、二阶互补滤波、卡尔曼滤波核心程序.docx（8页珍藏版）》请在第壹文秘上搜索。

1、一阶互补/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.enentary(f1.oatnewAng1.e,f1.oatnewRate,int100ptinie)(f1.oat.具体卡尔曼滤波*-4-indcnt-tabs-nxicf11c-basic-offsct

2、rX: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.ytrackedbythe

3、fi1.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.fee

4、1.freetosendmai1.Iothe11ui1.ing1.ist. autopi1.ot-,withquestionsaboutthiscode.* (c)2003Trarnme1.1.Hudson衾 *Thisparioftheautopi1.otOfIbOanIcodepackage. Autapi1.acisfreesoftware;youcanredistributeitndrormodify titUix1.erthetermsoftheGNUGenera1.Pub1.ic1.icenseaspub1.ishedby (heFreeSoftwareFoundation:eit

5、herversion2ofIhc1.icense,or (atyouroption)any1.aterversion., Autopi1.otisdistributedinthehopethatitwi1.1.beusefu1. butWITHOUTANYWARRANTY:withoutevenIhCimp1.iedwarrantyof MERCHANTABI1.ITYorHTNESSEORAPARneU1.ARPURPOSE.Seethe GNUGenera1.Pub1.ic1.icenseformoredetai1.s.*终Ywshou1.dhavereceivedacopyoftheGN

6、UGenera1.Pub1.ic1.icense a1.ongwithAutopi1.ot:ifnot.writetotheFreeSoftware Foundacion,Inc.,59Temp1.eP1.ace,Suite330.Bos1.on,MA021.1.1.-1.307USA#inc1.udcOurupdaterate.,Ihisishowoftenourstateisupdatedwithgyroratemeasurements.Fornow,WCdoiteverytimean8bitcounterrunningatC1.K/1024exis.Youwi1.1.havetochan

7、geIhisva1.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,w

8、ca1.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;* Qisa2x2matrixthatrcpr

9、cscntstheprocesscovariancenoise.* Inthiscase,itindicateshowmuchWe(rust(heacce1.e)nuer* tre1.ativeIoIhCgys./staticconstf1.oatQ.ang1.c=0.001;staticconstf1.oatQ_gyro=0.003;s(a(e_u|xiateisca1.1.edeverydtwithabiasedgyromeasuremenbytheuserofthemodu1.e.I1.updatestheCUrTUn1.ang1.eandrateestimate.JtThepitchg

10、yromeasunemshou1.dbesca1.edintora1.units,but* doesno1.needanybiasremova1.Thefi1.terwi1.1.Inickthebias.Ourstatevectoris:* X=Iang1.e.gyro_bias* 1(runsthestateestimationfbrardviathestatefunctions:分XXdo1.=Iang1.c_do1.gyro_bias_do* ang1.c-dH=gyro-gyro_biasjgyro_bias_doi=O* Andupdatesthecovariancematrixvi

11、athefunction:.* P(Jo=A*P+P*A+Q* AistheJacobianofXdowithrespecttothestates:* A=d(ang1.e_do(yd(ang1.e)dang1.e_do_bias_do1.Vd(ang1.e)d(gyro_bias_dot)/d(gyro_bias)* =IO-I1* (OOIyDuctothesma1.1.CPUavai1.ab1.eonthemicrocontro1.1.er,weve* handoptimizedtheCcodeIoon1.ycomputethetermsthatarc食exp1.kidynon-zex),a$we1.1.asexpandedoutIhematrix11th* 1.obedoneinasfewstepsaspossib1.e.Thisdemakeitharder* tor