惯性导航原理的理解

Part1.AccelerometerTounderstandthisunitwe'llstartwiththeaccelerometer.Whenthinkingaboutaccelerometersitisoftenusefultoimageaboxinshapeofacubewithaballinsideit.Youmayimaginesomethingelselikeacookieoradonut,butI'llimagineaball:了解本单元,我们从加速度计开始。当考虑加速度计通常想象成一个立方体形状的盒子里面有一个球很有助于理解。你也可以想像别的像饼干或者一个甜甜圈,但我会想象一个球:STILLWEIGTLES5STILLWEIGTLES5STATFZ=OgIfwetakethisboxinaplacewithnogravitationfieldsorforthatmatterwithnootherfieldsthatmightaffecttheball'sposition-theballwillsimplyfloatinthemiddleofthebox.Youcanimaginetheboxisinouter-spacefar-farawayfromanycosmicbodies,orifsuchaplaceishardtofindimagineatleastaspacecraftorbitingaroundtheplanetwhereeverythingisinweightlessstate.Fromthepictureaboveyoucanseethatweassigntoeachaxisapairofwalls(weremovedthewallY+sowecanlookinsidethebox).Imaginethateachwallispressuresensitive.Ifwemovesuddenlytheboxtotheleft(weaccelerateitwithaccelerationig=9.8m/s人2),theballwillhitthewallX-.Wethenmeasurethepressureforcethattheballappliestothewallandoutputavalueof-1gontheXaxis.

Pleasenotethattheaccelerometerwillactuallydetectaforcethatisdirectedintheoppositedirectionfromtheaccelerationvector.ThisforceisoftencalledInertialForceorFictitiousForce.Onethingyoushouldlearnfromthisisthatanaccelerometermeasuresaccelerationindirectlythroughaforcethatisappliedtooneofit'swalls(accordingtoourmodel,itmightbeaspringorsomethingelseinreallifeaccelerometers).Thisforcecanbecausedbytheacceleration,butaswe'llseeinthenextexampleitisnotalwayscausedbyacceleration.IfwetakeourmodelandputitonEarththeballwillfallontheZ-wallandwillapplyaforceof1gonthebottomwall,asshowninthepicturebelow:GROUNDInthiscasetheboxisn'tmovingbutwestillgetareadingof-1gontheZaxis.Thepressurethattheballhasappliedonthewallwascausedbyagravitationforce.Intheoryitcouldbeadifferenttypeofforce-forexample,ifyouimaginethatourballismetallic,placingamagnetnexttotheboxcouldmovetheballsoithitsanotherwall.Thiswassaidjusttoprovethatinessenceaccelerometermeasuresforcenotacceleration.Itjusthappensthataccelerationcausesaninertialforcethatiscapturedbytheforcedetectionmechanismoftheaccelerometer.WhilethismodelisnotexactlyhowaMEMSsensorisconstructeditisoftenusefulinsolvingaccelerometerrelatedproblems.Thereareactuallysimilarsensorsthathavemetallicballsinside,theyarecalledtiltswitches,howevertheyaremoreprimitiveandusuallytheycanonlytellifthedeviceisinclinedwithinsomerangeornot,nottheextentofinclination.Sofarwehaveanalyzedtheaccelerometeroutputonasingleaxisandthisisallyou'llgetwithasingleaxisaccelerometers.Therealvalueoftriaxialaccelerometerscomesfromthefactthattheycandetectinertialforcesonallthreeaxes.Let'sgobacktoourboxmodel,andlet'srotatethebox45degreestotheright.Theballwilltouch2wallsnow:Z-andX-asshowninthepicturebelow:^RAVITATONFOItCE^RAVITATONFOItCE9AGROUNDThevaluesof0.71arenotarbitrary,theyareactuallyanapproximationforSQRT(1/2).Thiswillbecomemoreclearasweintroduceournextmodelfortheaccelerometer.Inthepreviousmodelwehavefixedthegravitationforceandrotatedourimaginarybox.Inlast2exampleswehaveanalyzedtheoutputin2differentboxpositions,whiletheforcevectorremainedconstant.Whilethiswasusefulinunderstandinghowtheaccelerometerinteractswithoutsideforces,itismorepracticaltoperformcalculationsifwefixthecoordinatesystemtotheaxesoftheaccelerometerandimaginethattheforcevectorrotatesaroundus.Pleasehavealookatthemodelabove,Ipreservedthecolorsoftheaxessoyoucanmakeamentaltransitionfromthepreviousmodeltothenewone.Justimaginethateachaxisinthenewmodelisperpendiculartotherespectivefacesoftheboxinthepreviousmodel.ThevectorRistheforcevectorthattheaccelerometerismeasuring(itcouldbeeitherthegravitationforceortheinertialforcefromtheexamplesaboveoracombinationofboth).Rx,Ry,RzareprojectionoftheRvectorontheX,Y,Zaxes.Pleasenoticethefollowingrelation:R“2二Rx"2+Ry"2+Rz"2(Eq.1)whichisbasicallytheequivalent(PYtHaiGoreantheoremin3DRememberthatalittlebitearlierItoldyouthatthevaluesofSQRT(1/2)~0.71arenotrandom.IfyoupluGthemintheformulaabove,afterrecallinGthatourGravitationforcewas1Gwecanverifythat:「2=(-SQRT(1/2)厂2+0辽+(-SQRT(l/2)厂2simplybysubstitutinGR=1,Rx=-SQRT(1/2),Ry=0,Rz=-SQRT(1E/q2.)1inAfteralonGpreambleoftheorywe'reGettinGclosertoreallifeaccelerometers.ThevaluesRx,Ry,Rzareactuallylinearlyrelatedtothevaluesthatyourreal-lifeaccelerometerwilloutputandthatyoucanuseforperforminGvariouscalculations.BeforeweGettherelet'stalkalittleaboutthewayaccelerometerswilldeliverthisinformationtous.Mostaccelerometerswillfallintwocl大多受加S速度计可以分为以下两类):diGitalandanaloG.DiGitalaccelerometerswillGiveyouinformationusingaserialprot((®］议)likeI2C,SPIorUSART,whileanalogaccelerometerswilloutputavoltagelevelwithinapredefinedrangethatyouhavetoconverttoadigitalvalueusinganADC(analogtodigitalconverter)module.IwillnotgointomuchdetailabouthowADCworks,partlybecauseitissuchanextensivetopicandpartlybecauseitisdifferentfromoneplatformtoanother.Somemicrocontrollerwillhaveabuilt-inADCmodulessomeofthemwillneedexternalcomponentsinordertoperformtheADCconversions.NomatterwhattypeofADCmoduleyouuseyou'llendupwithavalueinacertainrange.Forexamplea10-bitADCmodulewilloutputavalueintherangeof0..1023,notethat1023=2人10-1.A12-bitADCmodulewilloutputavalueintherangeof0-4095,notethat4095=2人12-1.Let'smoveonbyconsideringasimpleexample,supposeour10bitADCmodulegaveusthefollowingvaluesforthethreeaccelerometerchannels(axes):AdcRx=586AdcRy=630AdcRz=561EachADCmodulewillhaveareferencevoltage,let'sassumeinourexampleitis3.3V.Toconverta10bitadcvaluetovoltageweusethefollowingformula:VoltsRx=AdcRx*Vref/1023Aquicknotehere:thatfor8bitADCthelastdividerwouldbe255=2人8-1,andfor12bitADClastdividerwouldbe4095=2人12-1.Applyingthisformulatoall3channelsweget:VoltsRx=586*3.3V/1023=~1.89V(weroundallresultsto2decimalpoints)VoltsRy=630*3.3V/1023=~2.03VVoltsRz=561*3.3V/1023=~1.81VEachaccelerometerhasazero-gvoltagelevel,youcanfinditinspecs,thisisthevoltagethatcorrespondsto0g.Togetasignedvoltagevalueweneedtocalculatetheshiftfromthislevel.Let'ssayour0gvoltagelevelisVzeroG=1.65V.Wecalculatethevoltageshiftsfromzero-gvoltageasfollows::DeltaVoltsRx=1.89V-1.65V=0.24VDeltaVoltsRy=2.03V一1.65V=0.38VDeltaVoltsRz=1.81V一1.65V=0.16VWenowhaveouraccelerometerreadingsinVolts,it'sstillnoting(9.8m/s人2),todothefinalconversionweapplytheaccelerometersensitivity,usuallyexpressedinmV/g.LetssayourSensitivity=478.5mV/g=0.4785V/g.Sensitivityvaluescanbefoundinaccelerometerspecifications(规格).Togetthefinalforcevaluesexpressedingweusethefollowingformula:Rx=DeltaVoltsRx/SensitivityRx=0.24V/0.4785V/g=~0.5gRy=0.38V/0.4785V/g=~0.79gRz=0.16V/0.4785V/g=~0.33gWecouldofcoursecombineallstepsinoneformula,butIwentthroughallthestepstomakeitclearhowyougofromADCreadingstoaforcevectorcomponentexpresseding.Rx=(AdcRx*Vref/1023-VzeroG)/Sensitivity(Eq.2)Ry=(AdcRy*Vref/1023-VzeroG)/SensitivityRz=(AdcRz*Vref/1023-VzeroG)/SensitivityWenowhaveall3componentsthatdefineourinertialforcevector,ifthedeviceisnotsubjecttootherforcesotherthangravitation,wecanassumethisisthedirectionofourgravitationforcevector.IfyouwanttocalculateinclinationofdevicerelativetothegroundyoucancalculatetheanglebetweenthisvectorandZaxis.Ifyouarealsointerestedinper-axisdirectionofinclinationyoucansplitthisresultinto2components:inclinationontheXandYaxisthatcanbecalculatedastheanglebetweengravitationvectorandX/Yaxes.Calculatingtheseanglesismoresimplethanyoumightthink,nowthatwehavecalculatedthevaluesforRx,RyandRz.Let'sgobacktoourlastaccelerometermodelanddosomeadditionalnotations:TheanglesthatweareinterestedinaretheanglesbetweenX,Y,ZaxesandtheforcevectorR.We'lldefinetheseanglesasAxr,Ayr,Azr.Youcannoticefromtheright-angletriangleformedbyRandRxthat:cos(Axr)=Rx/R,andsimilarly:cos(Ayr)=Ry/Rcos(Azr)=Rz/RWecandeductfromEq.1thatR=SQRT(Rx人2+Ry人2+Rz人2).Wecanfindnowouranglesbyusingarccos()function(theinversecos()function):Axr=arccos(Rx/R)Ayr=arccos(Ry/R)Azr=arccos(Rz/R)We'vegonealongwaytoexplaintheaccelerometermodel,justtocomeuptotheseformulas.Dependingonyourapplicationsyoumightwanttouseanyintermediateformulasthatwehavederived.We'llalsointroducethegyroscopemodelsoon,andwe'llseehowaccelerometerandgyroscopedatacanbecombinedtoprovideevenmoreaccurateinclinationestimations.Butbeforewedothatlet'sdosomemoreusefulnotations:cosX=cos(Axr)=Rx/RcosY=cos(Ayr)=Ry/RcosZ=cos(Azr)=Rz/RThistripletisoftencalledDirectionCosine,anditbasicallyrepresentstheunitvector(vectorwithlength1)thathassamedirectionasourRvector.Youcaneasilyverifythat:SQRT(cosX人2+cosY人2+cosZ人2)=1Thisisanicepropertysinceitabsolveusfrommonitoringthemodulus(length)ofRvector.Oftentimesifwe'rejustinterestedindirectionofourinertialvector,itmakessensetonormalizeit'smodulusinordertosimplifyothercalculations.Part2.GyroscopeWe'renotgoingtointroduceanyequivalentboxmodelforthegyroscopelikewedidforaccelerometer,insteadwe'regoingtojumpstraighttothesecondaccelerometermodelandwe'llshowwhatdoesthegyroscopemeasureaccordingtothismodel.Eachgyroscopechannelmeasurestherotationaroundoneoftheaxes.Forinstancea2-axesgyroscopewillmeasuretherotationaround(orsomemaysay"about")theXandYaxes.Toexpressthisrotationinnumberslet'sdosomenotations.Firstlet'sdefine:Rxz一istheprojectionoftheinertialforcevectorRontheXZplaneRyz一istheprojectionoftheinertialforcevectorRontheYZplaneFromtheright-angletriangleformedbyRxzandRz,usingPythagoreantheoremweget:Rxz人2=Rx人2+Rz人2,andsimilarly:Ryz人2=Ry人2+RzA2alsonotethat:Ra2=RxzA2+Ry人2,thiscanbederivedfromEq.landaboveequations,oritcanbederivedfromright-angletriangleformedbyRandRyzRa2=Ryz人2+RxA2We'renotgoingtousetheseformulasinthisarticlebutitisusefultonotetherelationbetweenallthevaluesinourmodel.Insteadwe'regoingtodefinetheanglebetweentheZaxisandRxz,Ryzvectorsasfollows:Axz一istheanglebetweentheRxz(projectionofRonXZplane)andZaxisAyz一istheanglebetweentheRyz(projectionofRonYZplane)andZaxisNowwe'regettingclosertowhatthegyroscopemeasures.Gyroscopemeasurestherateofchangesoftheanglesdefinedabove.Inotherwordsitwilloutputavaluethatislinearlyrelatedtotherateofchangeoftheseangles.Toexplainthislet'sassumethatwehavemeasuredtherotationanglearoundaxisY(thatwouldbeAxzangle)attimet0,andwedefineitasAxz0,nextwemeasuredthisangleatalatertimet1anditwasAxz1.Therateofchangewillbecalculatedasfollows:RateAxz=(Axzi一Axzo)/(ti一to).IfweexpressAxzindegrees,andtimeinseconds,thenthisvaluewillbeexpressedindeg/s.Thisiswhatagyroscopemeasures.Inpracticeagyroscope(unlessitisaspecialdigitalgyroscope)willrarelygiveyouavalueexpressedindeg/s.Sameasforaccelerometeryou'llgetanADCvaluethatyou'llneedtoconverttodeg/susingaformulasimilartoEq.2thatwehavedefinedforaccelerometer.Let'sintroducetheADCtodeg/sconversionformulaforgyroscope(weassumewe'reusinga1obitADCmodule,for8bitADCreplace1o23with255,for12bitADCreplace1o23with4o95).RateAxz=(AdcGyroXZ*Vref/1023-VzeroRate)/SensitivityEq.3RateAyz=(AdcGyroYZ*Vref/1023-VzeroRate)/SensitivityAdcGyroXZ,AdcGyroYZ+-areobtainedfromouradcmoduleandtheyrepresentthechannelsthatmeasuretherotationofprojectionofRvectorinXZrespectivelyinYZplanes,whichistheequivalenttosayingrotationwasdonearoundYandXaxesrespectively.Vref一istheADCreferencevoltagewe'lluse3.3VintheexamplebelowVzeroRate一isthezero-ratevoltage,inotherwordsthevoltagethatthegyroscopeoutputswhenitisnotsubjecttoanyrotation,fortheAcc_Gyroboarditisforexample1.23V(youcanfindthisvaluesinthespecs一butdon'ttrustthespecsmostgyroswillsufferslightoffsetafterbeingsolderedsomeasureVzeroRateforeachaxisoutputusingavoltmeter,usuallythisvaluewillnotchangeovertimeoncethegyrowassoldered,ifitvariates一writeacalibrationroutinetomeasureitbeforedevicestart-up,usermustbeinstructedtokeepdeviceinstillpositionuponstart-upforgyrostocalibrate).Sensitivity一isthesensitivityofyourgyroscopeitisexpressedinmV/(deg/s)oftenwrittenasmV/deg/s,itbasicallytellsyouhowmanymVwillthegyroscopeoutputincrease,ifyouincreasetherotationspeedbyonedeg/s.ThesensitivityofAccGyroboardisforexample2mV/deg/sor0.002V/deg/sLet'stakeanexample,supposeourADCmodulereturnedfollowingvalues:AdcGyroXZ=571AdcGyroXZ=323Usingtheaboveformula,andusingthespecsparametersofAcc_Gyroboardwe'llget:RateAxz=(571*3.3V/1023-1.23V)/(0.002V/deg/s)=~306deg/sRateAyz=(323*3.3V/1023一1.23V)/(0.002V/deg/s)=~-94deg/sInotherwordsthedevicerotatesaroundtheYaxis(orwecansayitrotatesinXZplane)withaspeedof306deg/sandaroundtheXaxis(orwecansayitrotatesinYZplane)withaspeedof-94deg/s.Pleasenotethatthenegativesignmeansthatthedevicerotatesintheoppositedirectionfromtheconventionalpositivedirection.Byconventiononedirectionofrotationispositive.Agoodgyroscopespecificationsheetwillshowyouwhichdirectionispositive,otherwiseyou'llhavetofinditbyexperimentingwiththedeviceandnotingwhichdirectionofrotationresultsinincreasingvoltageontheoutputpin.Thisisbestdoneusinganoscilloscopesinceassoonasyoustoptherotationthevoltagewilldropbacktothezero-ratelevel.Ifyou'reusingamultimeteryou'dhavetomaintainaconstantrotationrateforatleastfewsecondsandnotethevoltageduringthisrotation,thencompareitwiththezero-ratevoltage.Ifitisgreaterthanthezero-ratevoltageitmeansthatdirectionofrotationispositive.Part3.Puttingitalltogether.Combiningaccelerometerandgyroscopedata.Ifyou'rereadingthisarticleyouprobablyacquiredorareplanningtoacquireaIMUdevice,orprobablyyou'replanningtobuildonefromseparateaccelerometerandgyroscopedevices.NOTE:FORPRACTICALIMPLEMENTATIONANDTESTINGOFTHISALGORITHMPLEASEREADTHISARTICLE:/imu_kalman_arduino.htmlThefirststepinusingacombinationIMUdevicethatcombinesanaccelerometerandagyroscopeistoaligntheircoordinatesystems.Theeasiestwaytodoitistochoosethecoordinatesystemofaccelerometerasyourreferencecoordinatesystem.MostaccelerometerdatasheetswilldisplaythedirectionofX,Y,Zaxesrelativetotheimageofthephysicalchipordevice.ForexampleherearethedirectionsofX,Y,ZaxesasshowninspecificationsfortheAccGyroboard:Nextstepsare:一identifythegyroscopeoutputsthatcorrespondtoRateAxz,RateAyzvaluesdiscussedabove.一determineiftheseoutputsneedtobeinvertedduetophysicalpositionofgyroscoperelativetotheaccelerometer根据陀螺仪与加速度计的物理位置确定这些输出是否需要反转DonotassumethatifagyroscopehasanoutputmarkedXorY,itwillcorrespondtoanyaxisintheaccelerometercoordinatesystem,evenifthisoutputispartofanIMUunit.Thebestwayistotestit.不要认为陀螺仪输出带有X或Y标记，它将对应于加速度计坐标系的任何轴,即使这个输出是一个惯性传感器单元的一部分。最好的方法是测试它HereisasamplesequencetodeterminewhichoutputofgyroscopecorrespondstoRateAxzvaluediscussedabove.一startfromplacingthedeviceinhorizontalposition.BothXandYoutputsofaccelerometerwouldoutputthezero-gvoltage(forexampleforAcc_Gyroboardthisis1.65V)一nextstartrotatingthedevicearoundtheYaxis,anotherwaytosayitisthatyourotatethedeviceinXZplane,sothatXandZaccelerometeroutputschangeandYoutputremainsconstant.一whilerotatingthedeviceataconstantspeednotewhichgyroscopeoutputchanges,theothergyroscopeoutputsshouldremainconstant在以恒定速度旋转设备注意陀螺仪输出变化,另一个陀螺仪输出应该保持不变一thegyroscopeoutputthatchangedduringtherotationaroundYaxis(rotationinXZplane)willprovidetheinputvalueforAdcGyroXZ,fromwhichwecalculateRateAxz陀螺仪输出改变了在绕Y轴旋转(在XZ平面旋转)时,将提供AdcGyroXZ的输入值,从中我们计算RateAxz一thefinalstepistoensuretherotationdirectioncorrespondstoourmodel,insomecasesyoumayhavetoinverttheRateAxzvalueduetophysicalpositionofgyroscoperelativetotheaccelerometer最后一步是确保转动方向对应于我们的模型,在某些情况下,由于陀螺仪与加速度计的物理位置您可能必须转化RateAxz值一performagaintheabovetest,rotatingthedevicearoundtheYaxis,thistimemonitortheXoutputofaccelerometer(AdcRxinourmodel).IfAdcRxgrows(thefirst90degreesofrotationfromhorizontalposition),thenAdcGyroXZshoulddecrease.Thisisduetothefactthatwearemonitoringthegravitationvectorandwhendevicerotatesinonedirectionthevectorwillrotateinopositedirection(relativetothedevicecoordonatesystem,whichweareusing).So,otherwiseyouneedtoinvertRateAxz,youcanachievethisbyintroducingasignfactorinEq.3,asfollows:再次执行上面的操作，这次绕Y轴旋转设备，观察X加速度计的输出(在我们的模型是AdcRx的值)。如果AdcRx增大(第一个从水平位置旋转9。度),那么，AdcGyroXZ的值应该减少。这是由于这样的事实,我们正在观察重力向量,当设备在一个方向上旋转矢量旋转oposite方向(相对于设备coordonate系统,我们使用)。所以,否则你需要转化RateAxz，为此，可以引入一个标志在公式3中,如下：RateAxz=InvertAxz*(AdcGyroXZ*Vref/1023—VzeroRate)/Sensitivity,whereInvertAxzis1or-1sametestcanbedoneforRateAyz,byrotatingthedevicearoundtheXaxis,andyoucanidentifywhichgyroscopeoutputcorrespondstoRateAyz,andifitneedstobeinverted.OnceyouhavethevalueforInvertAyz,youshouldusethefollowingformulatocalculateRateAyz：RateAyz可以做相同的测试，绕X轴旋转设备，并且可以识别哪个陀螺仪输出对应于RateAyz,判断它是否需要反转。一旦得到InvertAyz的值，您应该使用下列公式计算RateAyz：RateAyz=InvertAyz*(AdcGyroYZ*Vref/1023-VzeroRate)/SensitivityIfyouwoulddothesetestsonAcc_Gyroboardyouwouldgetfollowingresults:一theoutputpinforRateAxzisGX4andInvertAxz=1一theoutputpinforRateAyzisGY4andInvertAyz=1Fromthispointonwe'llconsiderthatyouhavesetupyourIMUinsuchawaythatyoucancalculatecorrectvaluesforAxr,Ayr,Azr(asdefinedPart1.Accelerometer)andRateAxz,RateAyz(asdefinedinPart2.Gyroscope).Nextwe'llanalyzetherelationsbetweenthesevaluesthatturnoutusefulinobtainingmoreaccurateestimationoftheinclinationofthedevicerelativetothegroundplane.Youmightbeaskingyourselfbythispoint,ifaccelerometermodelalreadygaveusinclinationanglesofAxr,Ayr,Azrwhywouldwewanttobotherwiththegyroscopedata?Theanswerissimple:accelerometerdatacan'talwaysbetrusted100%.Thereareseveralreason,rememberthataccelerometermeasuresinertialforce(惯性力),suchaforcecanbecausedbygravitation(andideallyonlybygravitation),butitmightalsobecausedbyacceleration(movement)ofthedevice.Asaresultevenifaccelerometerisinarelativelystablestate,itisstillverysensitivetovibrationandmechanicalnoiseingeneral.（因此即使加速度计是在一个相对稳定的状态,它仍然是对振动和机械噪声非常敏感。）ThisisthemainreasonwhymostIMUsystemsuseagyroscopetosmoothoutanyaccelerometererrors.Buthowisthisdone?Andisthegyroscopefreefromnoise?Thegyroscopeisnotfreefromnoisehoweverbecauseitmeasuresrotationitislesssensitivetolinearmechanicalmovements,thetypeofnoisethataccelerometersuffersfrom,howevergyroscopeshaveothertypesofproblemslikeforexampledrift（notcomingbacktozero-ratevaluewhenrotationstops旋转停止回不到零值）.Neverthelessbyaveragingdatathatcomesfromaccelerometerandgyroscopewecanobtainarelativelybetterestimateofcurrentdeviceinclinationthanwewouldobtainbyusingtheaccelerometerdataalone.然而平均来自加速度计和陀螺仪的数据，我们可以获得一个相对更好的估计当前设备倾向，这比我们通过独自获得、使用加速度计数据更准确。InthenextstepsIwillintroduceanalgorithmthatwasinspiredbysomeideasusedinKalmanfilter（下一步，我将介绍一种受卡尔曼滤波算法启发的算法）,howeveritisbyfarmoresimpleandeasiertoimplementonembeddeddevices.（然而它是迄今为止更简单,更容易在嵌入式设备实现的算法）Beforethatlet'sseefirstwhatwewantouralgorithmtocalculate（在这之前，先让弄明白我们想让算法计算出什么的）.Well,itisthedirectionofgravitationforcevectorR=[Rx,Ry,Rz]fromwhichwecanderiveothervalueslikeAxr,Ayr,AzrorcosX,cosY,cosZthatwillgiveusanideaabouttheinclinationofourdevicerelativetothegroundplane,wediscusstherelationbetweenthesevaluesinPart1.（这是重力的方向向量R=（Rx,Ry,Rz）,我们可以获得其他值像Axr,Ayr,Azr或cosX,cosY,cosZ,这些值将给我们一个了解我们设备相对于地平面的倾斜程度，在第1部分中我们将讨论这些值之间的关系）Onemightsay-don'twealreadyhavethesevaluesRx,Ry,RzfromEq.2inPart1?（可能有人会说，我们不是已经从第一部分的式2中得到了Rx,Ry,Rz的值了吗）Wellyes,butrememberthatthesevaluesarederivedfromaccelerometerdataonly,soifyouwouldbetousethemdirectlyinyourapplicationyoumightgetmorenoisethanyourapplicationcantolerate.Toavoidfurtherconfusionlet'sre-definetheaccelerometermeasurementsasfollows:是的,但是请记住,这些值仅由加速度计数据,所以,如果你将在应用程序中直接使用它们的话，你可能遇到超出应用程序可以容忍噪声。为了避免进一步的混乱让我们重新定义加速度计测量如下:Racc一istheinertialforcevectorasmeasuredbyaccelerometer,thatconsistsoffollowingcomponents（projectionsonX,Y,Zaxes）:Racc 惯性力向量，由加速度计测量,包括以下分量（投影到X,Y,Z轴）：RxAcc=（AdcRx*Vref/1023-VzeroG）/SensitivityRyAcc=（AdcRy*Vref/1023-VzeroG）/SensitivityRzAcc=（AdcRz*Vref/1023-VzeroG）/SensitivitySofarwehaveasetofmeasuredvaluesthatwecanobtainpurelyfromaccelerometerADCvalues.We'llcallthissetofdataa"vector"andwe'llusethefollowingnotation.到目前为止,我们有一组从加速度计获得纯粹的ADC测量值。我们叫这组数据的“向量”,我们将使用以下符号。Racc=[RxAcc,RyAcc,RzAcc]BecausethesecomponentsofRacccanbeobtainedfromaccelerometerdata,wecanconsideritaninputtoouralgorithm.因为这些Racc的元素可以获得加速度计数据,我们可以把这些值当作的我们算法的一个输入。PleasenotethatbecauseRaccmeasuresthegravitationforceyou'llbecorrectifyouassumethatthelengthofthisvectordefinedasfollowsisequalorcloseto1g.|Racc|=SQRT(RxAcc人2+RyAcc人2+RzAcc人2),Howevertobesureitmakessensetoupdatethisvectorasfollows:不过可以肯定的更新如下这个向量是有意义的:Racc(normalized)=[RxAcc/|Racc|,RyAcc/|Racc|,RzAcc/|Racc|].ThiswillensurethelengthofyournormalizedRaccvectorisalways1.这将确保规范化Racc向量的长度总是1Nextwe'llintroduceanewvectorandwe'llcallit接下来,我们将介绍一种新的向量,我们就叫它Rest=[RxEst,RyEst,RzEst]Thiswillbetheoutputofouralgorithm,thesearecorrectedvaluesbasedongyroscopedataandbasedonpastestimateddata.这是我们的算法的输出,是用基于陀螺仪数据和基于过去的估计数据来纠正值。Hereiswhatouralgorithmwilldo:一accelerometertellsus:"YouarenowatpositionRacc"一wesay"Thankyou,butletmecheck",一thencorrectthisinformationwithgyroscopedataaswellaswithpastRestdataandweoutputanewestimatedvectorRest.然后用陀螺仪数据以及过去数据纠正这种信息与,输出一个新的估计向量一weconsiderResttobeour"bestbet"astothecurrentpositionofthedevice.我们认为Rest是我们设备当前位置的“最佳选择”Let'sseehowwecanmakeitwork.We'llstartoursequencebytrustingouraccelerometerandassigning:Rest(0)=Racc(0)BythewayrememberRestandRaccarevectors,sotheaboveequationisjustasimplewaytowrite3setsofequations,andavoidrepetition:记住Rest和Race都是向量，因此上述方程只是一种简单的方式来写3个方程，和避免重复:RxEst(0)=RxAcc(0)RyEst(0)=RyAcc(0)RzEst(0)=RzAcc(0)Nextwe'lldoregularmeasurementsatequaltimeintervalsofTseconds,andwe'llobtainnewmeasurementsthatwe'lldefineasRacc(1),Racc(2),Racc(3)andsoon.We'llalsoissuenewestimatesateachtimeintervalsRest(1),Rest(2),Rest(3)andsoon.接下来，我们将在相同时间间隔T秒做定期测量，和我们将获得新的测量值，我们将定义Racc(i),Racc(2),Racc(3)等等。我们还将每个时间间隔给出新的估计在Rest(i),Rest(2),Rest(3)等等。Supposewe'reatstepn.Wehavetwoknownsetsofvaluesthatwe'dliketouse:假设我们在步骤n。我们有两组已知的值，我们想使用：Rest(n-i)一ourpreviousestimate,withRest(o)=Racc(o)Racc(n)一ourcurrentaccelerometermeasurementBeforewecancalculateRest(n),let'sintroduceanewmeasuredvalue,thatwecanobtainfromourgyroscopeandapreviousestimate.我们可以计算Rest(n)之前，先介绍一种新的测量值,可以从陀螺仪和先前的估计获得。We'llcallitRgyro,anditisalsoavectorconsistingof3components：我们称之为Rgyro,也是一个向量由3元素组成：Rgyro=[RxGyro,RyGyro,RzGyro]We'llcalculatethisvectoronecomponentatatime.We'llstartwithRxGyro.Let'sstartbyobservingthefollowingrelationinourgyroscopemodel,fromtheright-angletriangleformedbyRzandRxzwecanderivethat:tan(Axz)=Rx/Rz=>Axz=atan2(Rx,Rz)Atan2mightbeafunctionyouneverusedbefore,itissimilartoatan,exceptitreturnsvaluesinrangeof(-PI,PI)asopposedto(-PI/2,PI/2)asreturnedbyatan,andittakes2argumentsinsteadofone.ItallowsustoconvertthetwovaluesofRx,Rztoanglesinthefullrangeof360degrees(-PItoPI).Youcanreadmoreaboutatan2here.SoknowingRxEst(n-1),andRzEst(n-1)wecanfind:Axz(n-1)=atan2(RxEst(n-1),RzEst(n-1)).RememberthatgyroscopemeasurestherateofchangeoftheAxzangle.SowecanestimatethenewangleAxz(n)asfollows:记住，陀螺仪测量的是Axz角变化率。所以我们可以估计新的角度Axz(n),如下：Axz(n)=Axz(n-1)+RateAxz(n)*TRememberthatRateAxzcanbeobtainedfromourgyroscopeADCreadings.Amorepreciseformulacanuseanaveragerotationratecalculatedasfollows：记住，我们可以从陀螺仪ADC读数获得RateAxZo可以使用一个更精确的平均转速公式,计算如下：RateAxzAvg=(RateAxz(n)+RateAxz(n-1))/2Axz(n)=Axz(n-1)+RateAxzAvg*TThesamewaywecanfind:Ayz(n)=Ayz(n-1)+RateAyz(n)*TOksonowwehaveAx