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* handoptimizedtheCcode100n1.ycomputethetermsthatarc食exp1.kidynon-zex)
12、a$we1.1.asexpandedoutIhematrix11th* 1.obedoneinasfewstepsaspossib1.e.Thisdemakeitharder* toread,debugandextend,buta1.soa1.1.owsustodothiswith* veryIiuIcCPUtime.阳voidstatc_updatc(COnStf1.oatq_m/Pitchgyromeasurement*)/Unbiasourgyro/constf1.oatq=q_m-CUbias：尸* ComputethederivativeofIhecovariancematrix*
13、 Pdot=A4P+PA,+QtWevehandcomiedIheexpansionof=(O110Omu1.tip1.ied* byPandPmu1.tip1.iedbyA,=(OO.-1.O.Thisisthenadded“tothediagona1.e1.ementsofQ.whichareQ_ang1.candQ_gyro.，/constf1.oatPdoI1.2Ascm11en1.edinstate_update.themathhereissimp1.ifiedto2=Q_ang1.e-I|OH1.j-P(1.|O|./0.0V-P1.1.11).*0J*/-1111.*h*/Qwy
14、eIJjV)：/Storeourunbiasgyroestimate*/rate=q:/* UpetermountedPerpendiCUIartoIherd(gyro_bia8)* =(o* becausetheang1.emeasurementdirect1.ycorrespondstotheang1.e* cs(inwtcandtheang1.emeasurementhasnore1.ationtothegyrobias.voidka1.man_up(1.atc(constf1.oatax_m,俨Xacce1.eration*/constoa1.az_m尸Zacce1.eru1.ion*
15、/)(/Computeourmeasuredang1.eandIheerrorinourestimate*7以下代码更改成白色，下*后复跳成其他吸色即可着到constf1.oatang1.c,m=atan2(-az_m,ax_m):consf1.oatang1.c-c=ang1.c_m-ang1.e;卢* C_()showshowthestatemeasurementdirect1.yre1.atesto* thestateestimate.* TheC_1showstha1.thes1.atemeasurementdocsnotre1.ate* tothegyrobiasestimate.W
16、Cdontactua1.1.yusethis,so* wecommentitout.Vconstf1.oatC0=1;*constf1.oatC_1=0;/卢* PC1.=P*C,.whichweusetwice.Thismakes* itworthwhi1.etoprecomputeandstore(hetwova1.ues.* NocthatC(0,1.=C_1.iszero,soWCdonotcomputeIhatenn.constf1.oatPCt)=CJ)P(0)0:Wefirstcompu1.ct=CRNote1.ha1.:+C-1.P1.OM1.=0*/constf1.oatPC
17、t_1.=C_0*P1.1.0*C1.*PI11I=O/* Computetheerrorestimate.FromtheKa1.manfi1.terpaper.京E=CPC+R* Dimensiona1.1.y,*京E=CPCR* Again,notethatC_1iszero,sowedonotcomputetheterm.*/constf1.oatE=R-ang1.e+CJ)PC1.o/C.1.*PCtJ=0*/* ComputetheKa1.manfi1.tergains.FromtheKa1.manpaper:* K=PCinv(E)* Dimensiona1.1.y:* K=PCi
18、nv(E)* 1.ucki1.1.y,Eis,so(hcinverseofEisjust1.E.constf1.oatK_0=PCt_O/E:constf1.oatK_J=PCt_1.fE;/* Updatecovariancena(rix.Again,fromtheKa1.manfi1.1.erpaper:* P=P-KCP.Dimensiona1.1.y:.* P-=KCP，t0,0=C0*PO,OC0J*P1.01* But.sinceCJiszero,wehave:* UOM三C0*P0,0=PCUo.Thissavesusai1.tingpointmu1.tip1.y.*/const
19、f1.oatt_()=PCtJ);/C_0*P1.ono1.+C，P1.1.110Tconstf1.oatt_1.=C_0*P01.;C.I*P1.111.=OVP0110-=KJ)*U);PtOHi-=Kau；P110.=K-I*1.0:P(IH-=KJ*ck片tUpdaIeours1.ateestimate.Agiiiikfrom(heKahnanpaper:*X+=Kerr*“And.dimensiona1.1.y.* X=X+Kcrr* eisaneasurenentof(hedif1.ejcein(heIneaSUredstate* and(heestimatesta1.e.Inourcase,i1.isjust1.hedifference* betweenthetwoaccc!erumctcrmeasuredang1.eandOUrestimatedang1.e.* /ang1.e+=K_0*ang1.e_errq_bias+=K_1*ang1.c_crr;
