/* Ċ Ġ* ĠCopyright Ġ( C ) Ġ 2 0 2 2 ĠAl berto ĠIr ur ue ta ĠCar ro Ġ( al berto @ ir ur ue ta . com ) Ċ Ġ* Ċ Ġ* ĠLicensed Ġunder Ġthe ĠApache ĠLicense , ĠVersion Ġ 2 . 0 Ġ( the Ġ" License "); Ċ Ġ* Ġyou Ġmay Ġnot Ġuse Ġthis Ġfile Ġexcept Ġin Ġcompliance Ġwith Ġthe ĠLicense . Ċ Ġ* ĠYou Ġmay Ġobtain Ġa Ġcopy Ġof Ġthe ĠLicense Ġat Ċ Ġ* Ċ Ġ* ĠĠĠĠĠĠĠĠ Ġhttp :// www . apache . org / licenses / LICENSE - 2 . 0 Ċ Ġ* Ċ Ġ* ĠUnless Ġrequired Ġby Ġapplicable Ġlaw Ġor Ġagreed Ġto Ġin Ġwriting , Ġsoftware Ċ Ġ* Ġdistributed Ġunder Ġthe ĠLicense Ġis Ġdistributed Ġon Ġan Ġ" AS ĠIS " ĠBASIS , Ċ Ġ* ĠWITHOUT ĠWARRANTIES ĠOR ĠCONDITIONS ĠOF ĠANY ĠKIND , Ġeither Ġexpress Ġor Ġimplied . Ċ Ġ* ĠSee Ġthe ĠLicense Ġfor Ġthe Ġspecific Ġlanguage Ġgoverning Ġpermissions Ġand Ċ Ġ* Ġlimitations Ġunder Ġthe ĠLicense . Ċ Ġ*/ Ċ package Ġcom . ir ur ue ta . android . navigation . in ert ial . calibration Ċ Ċ import Ġandroid . content . Context Ċ import Ġandroid . location . Location Ċ import Ġandroid . util . Log Ċ import Ġcom . ir ur ue ta . algebra . Matrix Ċ import Ġcom . ir ur ue ta . android . navigation . in ert ial . Gravity Helper Ċ import Ġcom . ir ur ue ta . android . navigation . in ert ial . calibration . builder . Accel erometer Internal Cal ibr ator Builder Ċ import Ġcom . ir ur ue ta . android . navigation . in ert ial . calibration . intervals . Accel erometer Interval Detector Ċ import Ġcom . ir ur ue ta . android . navigation . in ert ial . calibration . intervals . Interval Detector Ċ import Ġcom . ir ur ue ta . android . navigation . in ert ial . calibration . noise . Acc umulated Measurement Estimator Ċ import Ġcom . ir ur ue ta . android . navigation . in ert ial . calibration . noise . Gravity Norm Estimator Ċ import Ġcom . ir ur ue ta . android . navigation . in ert ial . calibration . noise . Stop Mode Ċ import Ġcom . ir ur ue ta . android . navigation . in ert ial . collect ors . Accel erometer Sensor Collector Ċ import Ġcom . ir ur ue ta . android . navigation . in ert ial . collect ors . Sensor Delay Ċ import Ġcom . ir ur ue ta . navigation . Navigation Exception Ċ import Ġcom . ir ur ue ta . navigation . in ert ial . Body Kin em atics Ċ import Ġcom . ir ur ue ta . navigation . in ert ial . calibration . Acceleration Tri ad Ċ import Ġcom . ir ur ue ta . navigation . in ert ial . calibration . Accel erometer Bias Un cert ainty Source Ċ import Ġcom . ir ur ue ta . navigation . in ert ial . calibration . Standard De viation Body Kin em atics Ċ import Ġcom . ir ur ue ta . navigation . in ert ial . calibration . accel erometer .* Ċ import Ġcom . ir ur ue ta . navigation . in ert ial . calibration . intervals . threshold factor . Default Accel erometer Quality Score Mapper Ċ import Ġcom . ir ur ue ta . navigation . in ert ial . calibration . intervals . threshold factor . Quality Score Mapper Ċ import Ġcom . ir ur ue ta . units . Acceleration Ċ import Ġcom . ir ur ue ta . units . Acceleration Unit Ċ import Ġkotlin . math . pow Ċ import Ġkotlin . math . sqrt Ċ Ċ /** Ċ Ġ* ĠCollect s Ġaccel erometer Ġmeasurements Ġby Ġdetect ing Ġperiods Ġwhen Ġdevice Ġremains Ġstatic , Ċ Ġ* Ġand Ġusing Ġsuch Ġstatic Ġperiods , Ġmeasurements Ġare Ġobtained Ġto Ġsolve Ġcalibration Ġparameters . Ċ Ġ* Ċ Ġ* Ġ@ property Ġcontext ĠAndroid Ġcontext . Ċ Ġ* Ġ@ property Ġsensor Type ĠOne Ġof Ġthe Ġsupported Ġaccel erometer Ġsensor Ġtypes . Ċ Ġ* Ġ@ property Ġsensor Delay ĠDelay Ġof Ġsensor Ġbetween Ġsamples . Ċ Ġ* Ġ@ property Ġsolve Calibration When En ough Measure ments Ġtrue Ġto Ġautomatically Ġsolve Ġcalibration Ġonce Ċ Ġ* Ġenough Ġmeasurements Ġare Ġavailable , Ġfalse Ġotherwise . Ċ Ġ* Ġ@ property Ġinitialization Started Listener Ġlistener Ġto Ġnotify Ġwhen Ġinitialization Ġstarts . Ċ Ġ* Ġ@ property Ġinitialization Completed Listener Ġlistener Ġto Ġnotify Ġwhen Ġinitialization Ġcompletes . Ċ Ġ* Ġ@ property Ġerror Listener Ġlistener Ġto Ġnotify Ġerrors . Ċ Ġ* Ġ@ property Ġun reli able Gravity Norm Estimation Listener Ġlistener Ġto Ġnotify Ġwhen Ġgravity Ġnorm Ġestimation Ċ Ġ* Ġbecomes Ġun reli able . ĠThis Ġis Ġonly Ġused Ġif Ġno Ġlocation Ġis Ġprovided . Ċ Ġ* Ġ@ property Ġinitial Bias Available Listener Ġlistener Ġto Ġnotify Ġwhen Ġa Ġguess Ġof Ġbias Ġvalues Ġis Ċ Ġ* Ġobtained . Ċ Ġ* Ġ@ property Ġnew Calibration Measurement Available Listener Ġlistener Ġto Ġnotify Ġwhen Ġa Ġnew Ġcalibration Ċ Ġ* Ġmeasurement Ġis Ġobtained . Ċ Ġ* Ġ@ property Ġready ToS olve Calibration Listener Ġlistener Ġto Ġnotify Ġwhen Ġcal ibr ator Ġis Ġready Ġto Ġbe Ċ Ġ* Ġsolved . Ċ Ġ* Ġ@ property Ġcalibration Sol ving Started Listener Ġlistener Ġto Ġnotify Ġwhen Ġcalibration Ġsolving Ġstarts . Ċ Ġ* Ġ@ property Ġcalibration Completed Listener Ġlistener Ġto Ġnotify Ġwhen Ġcalibration Ġis Ġsuccessfully Ċ Ġ* Ġcompleted . Ċ Ġ* Ġ@ property Ġstopped Listener Ġlistener Ġto Ġnotify Ġwhen Ġmeasurement Ġcollection Ġstops . Ċ Ġ* Ġ@ property Ġquality Score Mapper Ġmapper Ġto Ġconvert Ġcollected Ġmeasurements Ġinto Ġquality Ġscores , Ċ Ġ* Ġbased Ġon Ġthe Ġamount Ġof Ġstandard Ġdeviation Ġ( the Ġlarger Ġthe Ġvari ability , Ġthe Ġworse Ġthe Ġscore Ċ Ġ* Ġwill Ġbe ). Ċ Ġ*/ Ċ class ĠSingle Sensor Static Interval Accel erometer Cal ibr ator Ġprivate Ġconstructor ( ĊĠĠĠ Ġcontext : ĠContext , ĊĠĠĠ Ġval Ġsensor Type : ĠAcc eler ometer Sensor Collector . Sensor Type , ĊĠĠĠ Ġsensor Delay : ĠSensor Delay , ĊĠĠĠ Ġsolve Calibration When En ough Measure ments : ĠBoolean , ĊĠĠĠ Ġinitialization Started Listener : ĠOn Initialization Started Listener < Single Sensor Static Interval Accel erometer Cal ibr ator > ?, ĊĠĠĠ Ġinitialization Completed Listener : ĠOn Initialization Completed Listener < Single Sensor Static Interval Accel erometer Cal ibr ator > ?, ĊĠĠĠ Ġerror Listener : ĠOn Error Listener < Single Sensor Static Interval Accel erometer Cal ibr ator > ?, ĊĠĠĠ Ġvar Ġun reli able Gravity Norm Estimation Listener : ĠOn Un reli able Gravity Estimation Listener ?, ĊĠĠĠ Ġvar Ġinitial Bias Available Listener : ĠOn Initial Bias Available Listener ?, ĊĠĠĠ Ġnew Calibration Measurement Available Listener : ĠOn New Calibration Measurement Available Listener < Single Sensor Static Interval Accel erometer Cal ibr ator , ĠStandard De viation Body Kin em atics > ?, ĊĠĠĠ Ġready ToS olve Calibration Listener : ĠOn Ready ToS olve Calibration Listener < Single Sensor Static Interval Accel erometer Cal ibr ator > ?, ĊĠĠĠ Ġcalibration Sol ving Started Listener : ĠOn Calibration Sol ving Started Listener < Single Sensor Static Interval Accel erometer Cal ibr ator > ?, ĊĠĠĠ Ġcalibration Completed Listener : ĠOn Calibration Completed Listener < Single Sensor Static Interval Accel erometer Cal ibr ator > ?, ĊĠĠĠ Ġstopped Listener : ĠOn Stopped Listener < Single Sensor Static Interval Accel erometer Cal ibr ator > ?, ĊĠĠĠ Ġquality Score Mapper : ĠQuality Score Mapper < Standard De viation Body Kin em atics > Ċ ) Ġ: ĠSingle Sensor Static Interval Cal ibr ator < Single Sensor Static Interval Accel erometer Cal ibr ator , ĊĠĠĠĠĠĠĠ ĠStandard De viation Body Kin em atics , ĠAcc eler ometer Interval Detector , ĠAccel eration Unit , ĊĠĠĠĠĠĠĠ ĠAccel eration , ĠAccel eration Tri ad >( ĊĠĠĠ Ġcontext , ĊĠĠĠ Ġsensor Delay , Ġsolve Calibration When En ough Measure ments , Ġinitialization Started Listener , ĊĠĠĠ Ġinitialization Completed Listener , Ġerror Listener , Ġnew Calibration Measurement Available Listener , ĊĠĠĠ Ġready ToS olve Calibration Listener , Ġcalibration Sol ving Started Listener , ĊĠĠĠ Ġcalibration Completed Listener , Ġstopped Listener , Ġquality Score Mapper Ċ ) Ġ{ ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠConstructor . ĊĠĠĠĠ Ġ* ĊĠĠĠĠ Ġ* Ġ@ param Ġcontext ĠAndroid Ġcontext . ĊĠĠĠĠ Ġ* Ġ@ param Ġsensor Type ĠOne Ġof Ġthe Ġsupported Ġaccel erometer Ġsensor Ġtypes . ĊĠĠĠĠ Ġ* Ġ@ param Ġsensor Delay ĠDelay Ġof Ġsensor Ġbetween Ġsamples . ĊĠĠĠĠ Ġ* Ġ@ param Ġsolve Calibration When En ough Measure ments Ġtrue Ġto Ġautomatically Ġsolve Ġcalibration Ġonce ĊĠĠĠĠ Ġ* Ġenough Ġmeasurements Ġare Ġavailable , Ġfalse Ġotherwise . ĊĠĠĠĠ Ġ* Ġ@ param Ġis Ground Truth Initial Bias Ġtrue Ġif Ġestimated Ġbias Ġis Ġassumed Ġto Ġbe Ġthe Ġtrue Ġvalue , ĊĠĠĠĠ Ġ* Ġfalse Ġif Ġestimated Ġbias Ġis Ġassumed Ġto Ġbe Ġonly Ġan Ġinitial Ġguess . ĠWhen Ġ[ sensor Type ] Ġis ĊĠĠĠĠ Ġ* Ġ[ Accel erometer Sensor Collector . Sensor Type . AC CEL ERO METER ], Ġbias Ġguess Ġis Ġzero , ĊĠĠĠĠ Ġ* Ġotherwise Ġwhen Ġit Ġis Ġ[ Accel erometer Sensor Collector . Sensor Type . AC CEL ERO METER _ UN CAL IB R ATED ], ĊĠĠĠĠ Ġ* Ġbias Ġguess Ġis Ġthe Ġdevice Ġcal ibr ated Ġvalues . ĊĠĠĠĠ Ġ* Ġ@ param Ġlocation Ġlocation Ġwhere Ġdevice Ġis Ġlocated Ġat . ĠWhen Ġlocation Ġis Ġprovided , Ġgravity Ġnorm ĊĠĠĠĠ Ġ* Ġis Ġassumed Ġto Ġbe Ġthe Ġtheoret ical Ġvalue Ġdetermined Ġby ĠW GS 8 4 ĠEarth Ġmodel , Ġotherwise , Ġif Ġno ĊĠĠĠĠ Ġ* Ġlocation Ġis Ġprovided , Ġgravity Ġnorm Ġis Ġestimated Ġusing Ġa Ġgravity Ġsensor . ĊĠĠĠĠ Ġ* Ġ@ param Ġinitialization Started Listener Ġlistener Ġto Ġnotify Ġwhen Ġinitialization Ġstarts . ĊĠĠĠĠ Ġ* Ġ@ param Ġinitialization Completed Listener Ġlistener Ġto Ġnotify Ġwhen Ġinitialization Ġcompletes . ĊĠĠĠĠ Ġ* Ġ@ param Ġerror Listener Ġlistener Ġto Ġnotify Ġerrors . ĊĠĠĠĠ Ġ* Ġ@ param Ġun reli able Gravity Norm Estimation Listener Ġlistener Ġto Ġnotify Ġwhen Ġgravity Ġnorm ĊĠĠĠĠ Ġ* Ġestimation Ġbecomes Ġun reli able . ĠThis Ġis Ġonly Ġused Ġif Ġno Ġlocation Ġis Ġprovided . ĊĠĠĠĠ Ġ* Ġ@ param Ġinitial Bias Available Listener Ġlistener Ġto Ġnotify Ġwhen Ġa Ġguess Ġof Ġbias Ġvalues Ġis ĊĠĠĠĠ Ġ* Ġobtained . ĊĠĠĠĠ Ġ* Ġ@ param Ġnew Calibration Measurement Available Listener Ġlistener Ġto Ġnotify Ġwhen Ġa Ġnew Ġcalibration ĊĠĠĠĠ Ġ* Ġmeasurement Ġis Ġobtained . ĊĠĠĠĠ Ġ* Ġ@ param Ġready ToS olve Calibration Listener Ġlistener Ġto Ġnotify Ġwhen Ġcal ibr ator Ġis Ġready Ġto Ġbe ĊĠĠĠĠ Ġ* Ġsolved . ĊĠĠĠĠ Ġ* Ġ@ param Ġcalibration Sol ving Started Listener Ġlistener Ġto Ġnotify Ġwhen Ġcalibration Ġsolving Ġstarts . ĊĠĠĠĠ Ġ* Ġ@ param Ġcalibration Completed Listener Ġlistener Ġto Ġnotify Ġwhen Ġcalibration Ġis Ġsuccessfully ĊĠĠĠĠ Ġ* Ġcompleted . ĊĠĠĠĠ Ġ* Ġ@ param Ġstopped Listener Ġlistener Ġto Ġnotify Ġwhen Ġmeasurement Ġcollection Ġstops . ĊĠĠĠĠ Ġ* Ġ@ param Ġquality Score Mapper Ġmapper Ġto Ġconvert Ġcollected Ġmeasurements Ġinto Ġquality Ġscores , ĊĠĠĠĠ Ġ* Ġbased Ġon Ġthe Ġamount Ġof Ġstandard Ġdeviation Ġ( the Ġlarger Ġthe Ġvari ability , Ġthe Ġworse Ġthe Ġscore ĊĠĠĠĠ Ġ* Ġwill Ġbe ). ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġconstructor ( ĊĠĠĠĠĠĠĠ Ġcontext : ĠContext , ĊĠĠĠĠĠĠĠ Ġsensor Type : ĠAcc eler ometer Sensor Collector . Sensor Type Ġ= ĊĠĠĠĠĠĠĠĠĠĠĠ ĠAcc eler ometer Sensor Collector . Sensor Type . AC CEL ERO METER , ĊĠĠĠĠĠĠĠ Ġsensor Delay : ĠSensor Delay Ġ= ĠSensor Delay . FAST EST , ĊĠĠĠĠĠĠĠ Ġsolve Calibration When En ough Measure ments : ĠBoolean Ġ= Ġtrue , ĊĠĠĠĠĠĠĠ Ġis Ground Truth Initial Bias : ĠBoolean Ġ= Ġfalse , ĊĠĠĠĠĠĠĠ Ġlocation : ĠLocation ? Ġ= Ġnull , ĊĠĠĠĠĠĠĠ Ġinitialization Started Listener : ĠOn Initialization Started Listener < Single Sensor Static Interval Accel erometer Cal ibr ator >? Ġ= Ġnull , ĊĠĠĠĠĠĠĠ Ġinitialization Completed Listener : ĠOn Initialization Completed Listener < Single Sensor Static Interval Accel erometer Cal ibr ator >? Ġ= Ġnull , ĊĠĠĠĠĠĠĠ Ġerror Listener : ĠOn Error Listener < Single Sensor Static Interval Accel erometer Cal ibr ator >? Ġ= Ġnull , ĊĠĠĠĠĠĠĠ Ġun reli able Gravity Norm Estimation Listener : ĠOn Un reli able Gravity Estimation Listener ? Ġ= Ġnull , ĊĠĠĠĠĠĠĠ Ġinitial Bias Available Listener : ĠOn Initial Bias Available Listener ? Ġ= Ġnull , ĊĠĠĠĠĠĠĠ Ġnew Calibration Measurement Available Listener : ĠOn New Calibration Measurement Available Listener < Single Sensor Static Interval Accel erometer Cal ibr ator , ĠStandard De viation Body Kin em atics >? Ġ= Ġnull , ĊĠĠĠĠĠĠĠ Ġready ToS olve Calibration Listener : ĠOn Ready ToS olve Calibration Listener < Single Sensor Static Interval Accel erometer Cal ibr ator >? Ġ= Ġnull , ĊĠĠĠĠĠĠĠ Ġcalibration Sol ving Started Listener : ĠOn Calibration Sol ving Started Listener < Single Sensor Static Interval Accel erometer Cal ibr ator >? Ġ= Ġnull , ĊĠĠĠĠĠĠĠ Ġcalibration Completed Listener : ĠOn Calibration Completed Listener < Single Sensor Static Interval Accel erometer Cal ibr ator >? Ġ= Ġnull , ĊĠĠĠĠĠĠĠ Ġstopped Listener : ĠOn Stopped Listener < Single Sensor Static Interval Accel erometer Cal ibr ator >? Ġ= Ġnull , ĊĠĠĠĠĠĠĠ Ġquality Score Mapper : ĠQuality Score Mapper < Standard De viation Body Kin em atics > Ġ= ĠDefault Accel erometer Quality Score Mapper () ĊĠĠĠ Ġ) Ġ: Ġthis ( ĊĠĠĠĠĠĠĠ Ġcontext , ĊĠĠĠĠĠĠĠ Ġsensor Type , ĊĠĠĠĠĠĠĠ Ġsensor Delay , ĊĠĠĠĠĠĠĠ Ġsolve Calibration When En ough Measure ments , ĊĠĠĠĠĠĠĠ Ġinitialization Started Listener , ĊĠĠĠĠĠĠĠ Ġinitialization Completed Listener , ĊĠĠĠĠĠĠĠ Ġerror Listener , ĊĠĠĠĠĠĠĠ Ġun reli able Gravity Norm Estimation Listener , ĊĠĠĠĠĠĠĠ Ġinitial Bias Available Listener , ĊĠĠĠĠĠĠĠ Ġnew Calibration Measurement Available Listener , ĊĠĠĠĠĠĠĠ Ġready ToS olve Calibration Listener , ĊĠĠĠĠĠĠĠ Ġcalibration Sol ving Started Listener , ĊĠĠĠĠĠĠĠ Ġcalibration Completed Listener , ĊĠĠĠĠĠĠĠ Ġstopped Listener , ĊĠĠĠĠĠĠĠ Ġquality Score Mapper ĊĠĠĠ Ġ) Ġ{ ĊĠĠĠĠĠĠĠ Ġthis . is Ground Truth Initial Bias Ġ= Ġis Ground Truth Initial Bias ĊĠĠĠĠĠĠĠ Ġthis . location Ġ= Ġlocation ĊĠĠĠĠĠĠĠ Ġrequired Measure ments Ġ= Ġminimum Required Measure ments ĊĠĠĠĠĠĠĠ Ġrobust Pre limin ary Sub setSize Ġ= Ġminimum Required Measure ments ĊĠĠĠ Ġ} ĊĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠListener Ġused Ġby Ġinternal Ġinterval Ġdetector Ġto Ġhandle Ġevents Ġwhen Ġinitialization Ġis ĊĠĠĠĠ Ġ* Ġcompleted . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġoverride Ġval Ġinterval Detector Initialization Completed Listener Ġ= ĊĠĠĠĠĠĠĠ ĠInterval Detector . On Initialization Completed Listener < Accel erometer Interval Detector > Ġ{ Ġ_, Ġ_ Ġ-> ĊĠĠĠĠĠĠĠĠĠĠĠ Ġgravity Norm Ġ= Ġgravity Norm Estimator . average Norm ĊĠĠĠĠĠĠĠĠĠĠĠ Ġinitialization Completed Listener ?. on Initialization Completed ( ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġthis @ Single Sensor Static Interval Accel erometer Cal ibr ator ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ) ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠListener Ġused Ġby Ġthe Ġinternal Ġinterval Ġdetector Ġwhen Ġa Ġstatic Ġperiod Ġends Ġand Ġa Ġdynamic ĊĠĠĠĠ Ġ* Ġperiod Ġstarts . ĠThis Ġlistener Ġcontains Ġaccumulated Ġaccel erometer Ġaverage Ġvalues Ġduring Ġstatic ĊĠĠĠĠ Ġ* Ġperiod , Ġthat Ġwill Ġbe Ġused Ġas Ġa Ġmeasurement Ġto Ġsolve Ġcalibration . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġprivate Ġval Ġinterval Detector Dynamic Interval Detected Listener Ġ= ĊĠĠĠĠĠĠĠ ĠInterval Detector . On Dynamic Interval Detected Listener < Accel erometer Interval Detector > Ġ{ Ġ_, Ġ_, Ġ_, Ġ_, Ġ_, Ġ_, Ġ_, Ġaccumulated Avg X , Ġaccumulated Avg Y , Ġaccumulated Avg Z , Ġaccumulated Std X , Ġaccumulated Std Y , Ġaccumulated Std Z Ġ-> ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġadd Ġone Ġmeasurement ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġkin em atics Ġ= ĠBody Kin em atics ( acc umulated Avg X , Ġaccumulated Avg Y , Ġaccumulated Avg Z ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġstd Norm Ġ= Ġsqrt ( ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġaccumulated Std X . pow ( 2 . 0 ) Ġ+ Ġaccumulated Std Y . pow ( 2 . 0 ) Ġ+ Ġaccumulated Std Z . pow ( 2 . 0 ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġmeasurement Ġ= ĠStandard De viation Body Kin em atics ( kin em atics , Ġstd Norm , Ġ 0 . 0 ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġmeasurements . add ( measurement ) ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġreq Measure ments Ġ= ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġrequired Measure ments . coerce At Least ( minimum Required Measure ments ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġmeasurements Size Ġ= Ġmeasurements . size ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġnew Calibration Measurement Available Listener ?. on New Calibration Measurement Available ( ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġthis @ Single Sensor Static Interval Accel erometer Cal ibr ator , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġmeasurement , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġmeasurements Size , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġreq Measure ments ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ) ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġcheck Ġif Ġenough Ġmeasurements Ġhave Ġbeen Ġcollected ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġis Ready To Cal ibrate Ġ= Ġmeasurements Size Ġ>= Ġreq Measure ments ĊĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( is Ready To Cal ibrate ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġready ToS olve Calibration Listener ?. on Ready ToS olve Calibration ( ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġthis @ Single Sensor Static Interval Accel erometer Cal ibr ator ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ) ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġstop Ġinterval Ġdetector Ġsince Ġno Ġmore Ġmeasurements Ġneed Ġto Ġbe Ġcollected ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġinternal Stop ( true ) ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġbuild Ġcal ibr ator ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġinternal Cal ibr ator Ġ= Ġbuild Internal Cal ibr ator () ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( solve Calibration When En ough Measure ments ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġexecute Ġcalibration ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġinternal Cal ibrate () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠListener Ġfor Ġaccel erometer Ġsensor Ġcollector . ĊĠĠĠĠ Ġ* ĠThis Ġis Ġused Ġto Ġdetermine Ġdevice Ġcalibration Ġand Ġobtain Ġinitial Ġgues ses ĊĠĠĠĠ Ġ* Ġfor Ġaccel erometer Ġbias Ġ( only Ġavailable Ġif ĊĠĠĠĠ Ġ* Ġ[ Accel erometer Sensor Collector . Sensor Type . AC CEL ERO METER _ UN CAL IB R ATED ] Ġis Ġused , Ġotherwise Ġzero ĊĠĠĠĠ Ġ* Ġbias Ġis Ġassumed Ġas Ġan Ġinitial Ġguess ). ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġprivate Ġval Ġinterval Detector Measurement Listener Ġ= ĊĠĠĠĠĠĠĠ ĠAcc eler ometer Sensor Collector . On Measurement Listener Ġ{ Ġ_, Ġ_, Ġ_, Ġbx , Ġby , Ġb z , Ġ_, Ġ_ Ġ-> ĊĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( is First Measurement ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġupdate Initial Bi ases ( bx , Ġby , Ġb z ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠListener Ġfor Ġgravity Ġnorm Ġestimator . ĊĠĠĠĠ Ġ* ĠThis Ġis Ġused Ġto Ġapproximately Ġestimate Ġgravity Ġwhen Ġno Ġlocation Ġis Ġprovided , Ġby Ġusing Ġthe ĊĠĠĠĠ Ġ* Ġgravity Ġsensor Ġprovided Ġby Ġthe Ġdevice . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġprivate Ġval Ġgravity Norm Completed Listener Ġ= ĊĠĠĠĠĠĠĠ ĠAcc umulated Measurement Estimator . On Estimation Completed Listener < Gravity Norm Estimator > Ġ{ Ġestimator Ġ-> ĊĠĠĠĠĠĠĠĠĠĠĠ Ġgravity Norm Ġ= Ġestimator . average Norm ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠListener Ġto Ġhandle Ġevents Ġwhen Ġgravity Ġsensor Ġbecomes Ġun reli able . ĊĠĠĠĠ Ġ* ĠWhen Ġthis Ġhappens , Ġresult Ġof Ġcalibration Ġis Ġmarked Ġas Ġun reli able Ġand Ġshould Ġprobably Ġbe ĊĠĠĠĠ Ġ* Ġdiscarded . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġprivate Ġval Ġgravity Norm Un reli able Listener Ġ= ĊĠĠĠĠĠĠĠ ĠAcc umulated Measurement Estimator . On Un reli able Listener < Gravity Norm Estimator > Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġresult Un reli able Ġ= Ġtrue ĊĠĠĠĠĠĠĠĠĠĠĠ Ġun reli able Gravity Norm Estimation Listener ?. on Un reli able Gravity Estimation ( ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġthis @ Single Sensor Static Interval Accel erometer Cal ibr ator ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ) ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠInternal Ġinterval Ġdetector Ġto Ġdetect Ġperiods Ġwhen Ġdevice Ġremains Ġstatic . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġoverride Ġval Ġinterval Detector : ĠAcc eler ometer Interval Detector Ġ= ĊĠĠĠĠĠĠĠ ĠAcc eler ometer Interval Detector ( ĊĠĠĠĠĠĠĠĠĠĠĠ Ġcontext , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġsensor Type , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġsensor Delay , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġinterval Detector Initialization Started Listener , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġinterval Detector Initialization Completed Listener , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġinterval Detector Error Listener , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġdynamic Interval Detected Listener Ġ= Ġinterval Detector Dynamic Interval Detected Listener , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġmeasurement Listener Ġ= Ġinterval Detector Measurement Listener ĊĠĠĠĠĠĠĠ Ġ) ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGr avity Ġnorm Ġestimator . ĠIt Ġis Ġused Ġto Ġestimate Ġgravity Ġnorm Ġwhen Ġno Ġlocation Ġis Ġprovided . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġprivate Ġval Ġgravity Norm Estimator : ĠGr avity Norm Estimator Ġ= ĊĠĠĠĠĠĠĠ ĠGr avity Norm Estimator ( ĊĠĠĠĠĠĠĠĠĠĠĠ Ġcontext , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġsensor Delay , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġinitial Static Samples , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġstop Mode Ġ= ĠStop Mode . MAX _ SAMPLES _ ONLY , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġcompleted Listener Ġ= Ġgravity Norm Completed Listener , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġun reli able Listener Ġ= Ġgravity Norm Un reli able Listener ĊĠĠĠĠĠĠĠ Ġ) ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠInternal Ġcal ibr ator Ġused Ġto Ġsolve Ġthe Ġcalibration Ġparameters Ġonce Ġenough Ġmeasurements Ġare ĊĠĠĠĠ Ġ* Ġcollected Ġat Ġstatic Ġintervals . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġprivate Ġvar Ġinternal Cal ibr ator : ĠAcc eler ometer Non Linear Cal ibr ator ? Ġ= Ġnull ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠContains Ġgravity Ġnorm Ġ( either Ġobtained Ġby Ġthe Ġgravity Ġsensor , Ġor Ġdetermined Ġby Ġcurrent ĊĠĠĠĠ Ġ* Ġlocation Ġusing ĠW GS 8 4 ĠEarth Ġmodel ). ĠExp ressed Ġin Ġmeters Ġper Ġsquared Ġsecond Ġ( m / s ^ 2 ). ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġvar Ġgravity Norm : ĠDouble ? Ġ= Ġnull ĊĠĠĠĠĠĠĠ Ġprivate Ġset ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠIndicates Ġif Ġresult Ġis Ġun reli able . ĠThis Ġcan Ġhappen Ġif Ġno Ġlocation Ġis Ġprovided Ġand Ġgravity ĊĠĠĠĠ Ġ* Ġestimation Ġbecomes Ġun reli able . ĠWhen Ġthis Ġhappens Ġresult Ġof Ġcalibration Ġshould Ġprobably Ġbe ĊĠĠĠĠ Ġ* Ġdiscarded . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġvar Ġresult Un reli able Ġ= Ġfalse ĊĠĠĠĠĠĠĠ Ġprivate Ġset ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets ĠX - coordinate Ġof Ġbias Ġused Ġas Ġan Ġinitial Ġguess Ġand Ġexpressed Ġin Ġmeters Ġper Ġsquared Ġsecond ĊĠĠĠĠ Ġ* Ġ( m / s ^ 2 ). ĊĠĠĠĠ Ġ* ĠThis Ġvalue Ġis Ġdetermined Ġonce Ġthe Ġcal ibr ator Ġstarts . ĊĠĠĠĠ Ġ* ĠIf Ġ[ sensor Type ] Ġis Ġ[ Accel erometer Sensor Collector . Sensor Type . AC CEL ERO METER _ UN CAL IB R ATED ], Ġthis ĊĠĠĠĠ Ġ* Ġwill Ġbe Ġequal Ġto Ġthe Ġvalue Ġused Ġinternally Ġby Ġthe Ġdevice Ġas Ġpart Ġof Ġthe Ġaccel erometer ĊĠĠĠĠ Ġ* Ġhardware Ġcalibration . ĊĠĠĠĠ Ġ* ĠIf Ġ[ sensor Type ] Ġis Ġ[ Accel erometer Sensor Collector . Sensor Type . AC CEL ERO METER ], Ġthen ĊĠĠĠĠ Ġ* Ġaccel erometer Ġsensor Ġmeasurements Ġare Ġassumed Ġto Ġbe Ġalready Ġbias Ġcomp ens ated , Ġand Ġthe Ġinitial ĊĠĠĠĠ Ġ* Ġbias Ġis Ġassumed Ġto Ġbe Ġzero . ĊĠĠĠĠ Ġ* ĠIf Ġ[ is Ground Truth Initial Bias ] Ġis Ġtrue , Ġthis Ġis Ġassumed Ġto Ġbe Ġthe Ġtrue Ġbias , Ġand Ġ[ estimated Bias X ] Ġwill Ġbe ĊĠĠĠĠ Ġ* Ġequal Ġto Ġthis Ġvalue , Ġotherwise Ġ[ estimated Bias X ] Ġwill Ġbe Ġthe Ġestimated Ġbias Ġafter Ġsolving ĊĠĠĠĠ Ġ* Ġcalibration , Ġwhich Ġwill Ġdiffer Ġfrom Ġ[ estimated Bias X ]. ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġvar Ġinitial Bias X : ĠDouble ? Ġ= Ġnull ĊĠĠĠĠĠĠĠ Ġprivate Ġset ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets ĠY - coordinate Ġof Ġbias Ġused Ġas Ġan Ġinitial Ġguess Ġand Ġexpressed Ġin Ġmeters Ġper Ġsquared Ġsecond ĊĠĠĠĠ Ġ* Ġ( m / s ^ 2 ). ĊĠĠĠĠ Ġ* ĠThis Ġvalue Ġis Ġdetermined Ġonce Ġthe Ġcal ibr ator Ġstarts . ĊĠĠĠĠ Ġ* ĠIf Ġ[ sensor Type ] Ġis Ġ[ Accel erometer Sensor Collector . Sensor Type . AC CEL ERO METER _ UN CAL IB R ATED ], Ġthis ĊĠĠĠĠ Ġ* Ġwill Ġbe Ġequal Ġto Ġthe Ġvalue Ġused Ġinternally Ġby Ġthe Ġdevice Ġas Ġpart Ġof Ġthe Ġaccel erometer ĊĠĠĠĠ Ġ* Ġhardware Ġcalibration . ĊĠĠĠĠ Ġ* ĠIf Ġ[ sensor Type ] Ġis Ġ[ Accel erometer Sensor Collector . Sensor Type . AC CEL ERO METER ], Ġthen ĊĠĠĠĠ Ġ* Ġaccel erometer Ġsensor Ġmeasurements Ġare Ġassumed Ġto Ġbe Ġalready Ġbias Ġcomp ens ated , Ġand Ġthe Ġinitial ĊĠĠĠĠ Ġ* Ġbias Ġis Ġassumed Ġto Ġbe Ġzero . ĊĠĠĠĠ Ġ* ĠIf Ġ[ is Ground Truth Initial Bias ] Ġis Ġtrue , Ġthis Ġis Ġassumed Ġto Ġbe Ġthe Ġtrue Ġbias , Ġand Ġ[ estimated Bias Y ] Ġwill Ġbe ĊĠĠĠĠ Ġ* Ġequal Ġto Ġthis Ġvalue , Ġotherwise Ġ[ estimated Bias Y ] Ġwill Ġbe Ġthe Ġestimated Ġbias Ġafter Ġsolving ĊĠĠĠĠ Ġ* Ġcalibration , Ġwhich Ġwill Ġdiffer Ġfrom Ġ[ estimated Bias Y ]. ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġvar Ġinitial Bias Y : ĠDouble ? Ġ= Ġnull ĊĠĠĠĠĠĠĠ Ġprivate Ġset ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets ĠZ - coordinate Ġof Ġbias Ġused Ġas Ġan Ġinitial Ġguess Ġand Ġexpressed Ġin Ġmeters Ġper Ġsquared Ġsecond ĊĠĠĠĠ Ġ* Ġ( m / s ^ 2 ). ĊĠĠĠĠ Ġ* ĠThis Ġvalue Ġis Ġdetermined Ġonce Ġthe Ġcal ibr ator Ġstarts . ĊĠĠĠĠ Ġ* ĠIf Ġ[ sensor Type ] Ġis Ġ[ Accel erometer Sensor Collector . Sensor Type . AC CEL ERO METER _ UN CAL IB R ATED ], Ġthis ĊĠĠĠĠ Ġ* Ġwill Ġbe Ġequal Ġto Ġthe Ġvalue Ġused Ġinternally Ġby Ġthe Ġdevice Ġas Ġpart Ġof Ġthe Ġaccel erometer ĊĠĠĠĠ Ġ* Ġhardware Ġcalibration . ĊĠĠĠĠ Ġ* ĠIf Ġ[ sensor Type ] Ġis Ġ[ Accel erometer Sensor Collector . Sensor Type . AC CEL ERO METER ], Ġthen ĊĠĠĠĠ Ġ* Ġaccel erometer Ġsensor Ġmeasurements Ġare Ġassumed Ġto Ġbe Ġalready Ġbias Ġcomp ens ated , Ġand Ġthe Ġinitial ĊĠĠĠĠ Ġ* Ġbias Ġis Ġassumed Ġto Ġbe Ġzero . ĊĠĠĠĠ Ġ* ĠIf Ġ[ is Ground Truth Initial Bias ] Ġis Ġtrue , Ġthis Ġis Ġassumed Ġto Ġbe Ġthe Ġtrue Ġbias , Ġand Ġ[ estimated Bias Z ] Ġwill Ġbe ĊĠĠĠĠ Ġ* Ġequal Ġto Ġthis Ġvalue , Ġotherwise Ġ[ estimated Bias Z ] Ġwill Ġbe Ġthe Ġestimated Ġbias Ġafter Ġsolving ĊĠĠĠĠ Ġ* Ġcalibration , Ġwhich Ġwill Ġdiffer Ġfrom Ġ[ estimated Bias Z ]. ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġvar Ġinitial Bias Z : ĠDouble ? Ġ= Ġnull ĊĠĠĠĠĠĠĠ Ġprivate Ġset ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets ĠX - coordinate Ġof Ġbias Ġused Ġas Ġan Ġinitial Ġguess . ĊĠĠĠĠ Ġ* ĠThis Ġvalue Ġis Ġdetermined Ġonce Ġthe Ġcal ibr ator Ġstarts . ĊĠĠĠĠ Ġ* ĠIf Ġ[ sensor Type ] Ġis Ġ[ Accel erometer Sensor Collector . Sensor Type . AC CEL ERO METER _ UN CAL IB R ATED ], Ġthis ĊĠĠĠĠ Ġ* Ġwill Ġbe Ġequal Ġto Ġthe Ġvalue Ġused Ġinternally Ġby Ġthe Ġdevice Ġas Ġpart Ġof Ġthe Ġaccel erometer ĊĠĠĠĠ Ġ* Ġhardware Ġcalibration . ĊĠĠĠĠ Ġ* ĠIf Ġ[ sensor Type ] Ġis Ġ[ Accel erometer Sensor Collector . Sensor Type . AC CEL ERO METER ], Ġthen ĊĠĠĠĠ Ġ* Ġaccel erometer Ġsensor Ġmeasurements Ġare Ġassumed Ġto Ġbe Ġalready Ġbias Ġcomp ens ated , Ġand Ġthe Ġinitial ĊĠĠĠĠ Ġ* Ġbias Ġis Ġassumed Ġto Ġbe Ġzero . ĊĠĠĠĠ Ġ* ĠIf Ġ[ is Ground Truth Initial Bias ] Ġis Ġtrue , Ġthis Ġis Ġassumed Ġto Ġbe Ġthe Ġtrue Ġbias , Ġand Ġ[ estimated Bias X ] Ġwill Ġbe ĊĠĠĠĠ Ġ* Ġequal Ġto Ġthis Ġvalue , Ġotherwise Ġ[ estimated Bias X ] Ġwill Ġbe Ġthe Ġestimated Ġbias Ġafter Ġsolving ĊĠĠĠĠ Ġ* Ġcalibration , Ġwhich Ġwill Ġdiffer Ġfrom Ġ[ estimated Bias X ]. ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġval Ġinitial Bias X As Measurement : ĠAccel eration ? ĊĠĠĠĠĠĠĠ Ġget () Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġinitial Bias X Ġ= Ġthis . initial Bias X Ġ?: Ġreturn Ġnull ĊĠĠĠĠĠĠĠĠĠĠĠ Ġreturn ĠAccel eration ( initial Bias X , ĠAccel eration Unit . METER S _ PER _ S QUARE D _ SECOND ) ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets ĠX - coordinate Ġof Ġbias Ġused Ġas Ġan Ġinitial Ġguess . ĊĠĠĠĠ Ġ* ĠThis Ġvalue Ġis Ġdetermined Ġonce Ġthe Ġcal ibr ator Ġstarts . ĊĠĠĠĠ Ġ* ĠIf Ġ[ sensor Type ] Ġis Ġ[ Accel erometer Sensor Collector . Sensor Type . AC CEL ERO METER _ UN CAL IB R ATED ], Ġthis ĊĠĠĠĠ Ġ* Ġwill Ġbe Ġequal Ġto Ġthe Ġvalue Ġused Ġinternally Ġby Ġthe Ġdevice Ġas Ġpart Ġof Ġthe Ġaccel erometer ĊĠĠĠĠ Ġ* Ġhardware Ġcalibration . ĊĠĠĠĠ Ġ* ĠIf Ġ[ sensor Type ] Ġis Ġ[ Accel erometer Sensor Collector . Sensor Type . AC CEL ERO METER ], Ġthen ĊĠĠĠĠ Ġ* Ġaccel erometer Ġsensor Ġmeasurements Ġare Ġassumed Ġto Ġbe Ġalready Ġbias Ġcomp ens ated , Ġand Ġthe Ġinitial ĊĠĠĠĠ Ġ* Ġbias Ġis Ġassumed Ġto Ġbe Ġzero . ĊĠĠĠĠ Ġ* ĠIf Ġ[ is Ground Truth Initial Bias ] Ġis Ġtrue , Ġthis Ġis Ġassumed Ġto Ġbe Ġthe Ġtrue Ġbias , Ġand Ġ[ estimated Bias X ] Ġwill Ġbe ĊĠĠĠĠ Ġ* Ġequal Ġto Ġthis Ġvalue , Ġotherwise Ġ[ estimated Bias X ] Ġwill Ġbe Ġthe Ġestimated Ġbias Ġafter Ġsolving ĊĠĠĠĠ Ġ* Ġcalibration , Ġwhich Ġwill Ġdiffer Ġfrom Ġ[ estimated Bias X ]. ĊĠĠĠĠ Ġ* ĊĠĠĠĠ Ġ* Ġ@ param Ġresult Ġinstance Ġwhere Ġresult Ġwill Ġbe Ġstored . ĊĠĠĠĠ Ġ* Ġ@ return Ġtrue Ġif Ġinitial Ġbias Ġis Ġavailable , Ġfalse Ġotherwise . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġget Initial Bias X As Measurement ( result : ĠAccel eration ): ĠBoolean Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġinitial Bias X Ġ= Ġthis . initial Bias X ĊĠĠĠĠĠĠĠ Ġreturn Ġif Ġ( initial Bias X Ġ!= Ġnull ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġresult . value Ġ= Ġinitial Bias X ĊĠĠĠĠĠĠĠĠĠĠĠ Ġresult . unit Ġ= ĠAccel eration Unit . METER S _ PER _ S QUARE D _ SECOND ĊĠĠĠĠĠĠĠĠĠĠĠ Ġtrue ĊĠĠĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġfalse ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets ĠY - coordinate Ġof Ġbias Ġused Ġas Ġan Ġinitial Ġguess . ĊĠĠĠĠ Ġ* ĠThis Ġvalue Ġis Ġdetermined Ġonce Ġthe Ġcal ibr ator Ġstarts . ĊĠĠĠĠ Ġ* ĠIf Ġ[ sensor Type ] Ġis Ġ[ Accel erometer Sensor Collector . Sensor Type . AC CEL ERO METER _ UN CAL IB R ATED ], Ġthis ĊĠĠĠĠ Ġ* Ġwill Ġbe Ġequal Ġto Ġthe Ġvalue Ġused Ġinternally Ġby Ġthe Ġdevice Ġas Ġpart Ġof Ġthe Ġaccel erometer ĊĠĠĠĠ Ġ* Ġhardware Ġcalibration . ĊĠĠĠĠ Ġ* ĠIf Ġ[ sensor Type ] Ġis Ġ[ Accel erometer Sensor Collector . Sensor Type . AC CEL ERO METER ], Ġthen ĊĠĠĠĠ Ġ* Ġaccel erometer Ġsensor Ġmeasurements Ġare Ġassumed Ġto Ġbe Ġalready Ġbias Ġcomp ens ated , Ġand Ġthe Ġinitial ĊĠĠĠĠ Ġ* Ġbias Ġis Ġassumed Ġto Ġbe Ġzero . ĊĠĠĠĠ Ġ* ĠIf Ġ[ is Ground Truth Initial Bias ] Ġis Ġtrue , Ġthis Ġis Ġassumed Ġto Ġbe Ġthe Ġtrue Ġbias , Ġand Ġ[ estimated Bias Y ] Ġwill Ġbe ĊĠĠĠĠ Ġ* Ġequal Ġto Ġthis Ġvalue , Ġotherwise Ġ[ estimated Bias Y ] Ġwill Ġbe Ġthe Ġestimated Ġbias Ġafter Ġsolving ĊĠĠĠĠ Ġ* Ġcalibration , Ġwhich Ġwill Ġdiffer Ġfrom Ġ[ estimated Bias Y ]. ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġval Ġinitial Bias Y As Measurement : ĠAccel eration ? ĊĠĠĠĠĠĠĠ Ġget () Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġinitial Bias Y Ġ= Ġthis . initial Bias Y Ġ?: Ġreturn Ġnull ĊĠĠĠĠĠĠĠĠĠĠĠ Ġreturn ĠAccel eration ( initial Bias Y , ĠAccel eration Unit . METER S _ PER _ S QUARE D _ SECOND ) ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets ĠY - coordinate Ġof Ġbias Ġused Ġas Ġan Ġinitial Ġguess . ĊĠĠĠĠ Ġ* ĠThis Ġvalue Ġis Ġdetermined Ġonce Ġthe Ġcal ibr ator Ġstarts . ĊĠĠĠĠ Ġ* ĠIf Ġ[ sensor Type ] Ġis Ġ[ Accel erometer Sensor Collector . Sensor Type . AC CEL ERO METER _ UN CAL IB R ATED ], Ġthis ĊĠĠĠĠ Ġ* Ġwill Ġbe Ġequal Ġto Ġthe Ġvalue Ġused Ġinternally Ġby Ġthe Ġdevice Ġas Ġpart Ġof Ġthe Ġaccel erometer ĊĠĠĠĠ Ġ* Ġhardware Ġcalibration . ĊĠĠĠĠ Ġ* ĠIf Ġ[ sensor Type ] Ġis Ġ[ Accel erometer Sensor Collector . Sensor Type . AC CEL ERO METER ], Ġthen ĊĠĠĠĠ Ġ* Ġaccel erometer Ġsensor Ġmeasurements Ġare Ġassumed Ġto Ġbe Ġalready Ġbias Ġcomp ens ated , Ġand Ġthe Ġinitial ĊĠĠĠĠ Ġ* Ġbias Ġis Ġassumed Ġto Ġbe Ġzero . ĊĠĠĠĠ Ġ* ĠIf Ġ[ is Ground Truth Initial Bias ] Ġis Ġtrue , Ġthis Ġis Ġassumed Ġto Ġbe Ġthe Ġtrue Ġbias , Ġand Ġ[ estimated Bias Y ] Ġwill Ġbe ĊĠĠĠĠ Ġ* Ġequal Ġto Ġthis Ġvalue , Ġotherwise Ġ[ estimated Bias Y ] Ġwill Ġbe Ġthe Ġestimated Ġbias Ġafter Ġsolving ĊĠĠĠĠ Ġ* Ġcalibration , Ġwhich Ġwill Ġdiffer Ġfrom Ġ[ estimated Bias Y ]. ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġget Initial Bias Y As Measurement ( result : ĠAccel eration ): ĠBoolean Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġinitial Bias Y Ġ= Ġthis . initial Bias Y ĊĠĠĠĠĠĠĠ Ġreturn Ġif Ġ( initial Bias Y Ġ!= Ġnull ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġresult . value Ġ= Ġinitial Bias Y ĊĠĠĠĠĠĠĠĠĠĠĠ Ġresult . unit Ġ= ĠAccel eration Unit . METER S _ PER _ S QUARE D _ SECOND ĊĠĠĠĠĠĠĠĠĠĠĠ Ġtrue ĊĠĠĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġfalse ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets ĠZ - coordinate Ġof Ġbias Ġused Ġas Ġan Ġinitial Ġguess . ĊĠĠĠĠ Ġ* ĠThis Ġvalue Ġis Ġdetermined Ġonce Ġthe Ġcal ibr ator Ġstarts . ĊĠĠĠĠ Ġ* ĠIf Ġ[ sensor Type ] Ġis Ġ[ Accel erometer Sensor Collector . Sensor Type . AC CEL ERO METER _ UN CAL IB R ATED ], Ġthis ĊĠĠĠĠ Ġ* Ġwill Ġbe Ġequal Ġto Ġthe Ġvalue Ġused Ġinternally Ġby Ġthe Ġdevice Ġas Ġpart Ġof Ġthe Ġaccel erometer ĊĠĠĠĠ Ġ* Ġhardware Ġcalibration . ĊĠĠĠĠ Ġ* ĠIf Ġ[ sensor Type ] Ġis Ġ[ Accel erometer Sensor Collector . Sensor Type . AC CEL ERO METER ], Ġthen ĊĠĠĠĠ Ġ* Ġaccel erometer Ġsensor Ġmeasurements Ġare Ġassumed Ġto Ġbe Ġalready Ġbias Ġcomp ens ated , Ġand Ġthe Ġinitial ĊĠĠĠĠ Ġ* Ġbias Ġis Ġassumed Ġto Ġbe Ġzero . ĊĠĠĠĠ Ġ* ĠIf Ġ[ is Ground Truth Initial Bias ] Ġis Ġtrue , Ġthis Ġis Ġassumed Ġto Ġbe Ġthe Ġtrue Ġbias , Ġand Ġ[ estimated Bias Z ] Ġwill Ġbe ĊĠĠĠĠ Ġ* Ġequal Ġto Ġthis Ġvalue , Ġotherwise Ġ[ estimated Bias Z ] Ġwill Ġbe Ġthe Ġestimated Ġbias Ġafter Ġsolving ĊĠĠĠĠ Ġ* Ġcalibration , Ġwhich Ġwill Ġdiffer Ġfrom Ġ[ estimated Bias Z ]. ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġval Ġinitial Bias Z As Measurement : ĠAccel eration ? ĊĠĠĠĠĠĠĠ Ġget () Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġinitial Bias Z Ġ= Ġthis . initial Bias Z Ġ?: Ġreturn Ġnull ĊĠĠĠĠĠĠĠĠĠĠĠ Ġreturn ĠAccel eration ( initial Bias Z , ĠAccel eration Unit . METER S _ PER _ S QUARE D _ SECOND ) ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets ĠZ - coordinate Ġof Ġbias Ġused Ġas Ġan Ġinitial Ġguess . ĊĠĠĠĠ Ġ* ĠThis Ġvalue Ġis Ġdetermined Ġonce Ġthe Ġcal ibr ator Ġstarts . ĊĠĠĠĠ Ġ* ĠIf Ġ[ sensor Type ] Ġis Ġ[ Accel erometer Sensor Collector . Sensor Type . AC CEL ERO METER _ UN CAL IB R ATED ], Ġthis ĊĠĠĠĠ Ġ* Ġwill Ġbe Ġequal Ġto Ġthe Ġvalue Ġused Ġinternally Ġby Ġthe Ġdevice Ġas Ġpart Ġof Ġthe Ġaccel erometer ĊĠĠĠĠ Ġ* Ġhardware Ġcalibration . ĊĠĠĠĠ Ġ* ĠIf Ġ[ sensor Type ] Ġis Ġ[ Accel erometer Sensor Collector . Sensor Type . AC CEL ERO METER ], Ġthen ĊĠĠĠĠ Ġ* Ġaccel erometer Ġsensor Ġmeasurements Ġare Ġassumed Ġto Ġbe Ġalready Ġbias Ġcomp ens ated , Ġand Ġthe Ġinitial ĊĠĠĠĠ Ġ* Ġbias Ġis Ġassumed Ġto Ġbe Ġzero . ĊĠĠĠĠ Ġ* ĠIf Ġ[ is Ground Truth Initial Bias ] Ġis Ġtrue , Ġthis Ġis Ġassumed Ġto Ġbe Ġthe Ġtrue Ġbias , Ġand Ġ[ estimated Bias Z ] Ġwill Ġbe ĊĠĠĠĠ Ġ* Ġequal Ġto Ġthis Ġvalue , Ġotherwise Ġ[ estimated Bias Z ] Ġwill Ġbe Ġthe Ġestimated Ġbias Ġafter Ġsolving ĊĠĠĠĠ Ġ* Ġcalibration , Ġwhich Ġwill Ġdiffer Ġfrom Ġ[ estimated Bias Z ]. ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġget Initial Bias Z As Measurement ( result : ĠAccel eration ): ĠBoolean Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġinitial Bias Z Ġ= Ġthis . initial Bias Z ĊĠĠĠĠĠĠĠ Ġreturn Ġif Ġ( initial Bias Z Ġ!= Ġnull ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġresult . value Ġ= Ġinitial Bias Z ĊĠĠĠĠĠĠĠĠĠĠĠ Ġresult . unit Ġ= ĠAccel eration Unit . METER S _ PER _ S QUARE D _ SECOND ĊĠĠĠĠĠĠĠĠĠĠĠ Ġtrue ĊĠĠĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġfalse ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġinitial Ġbias Ġcoordinates Ġused Ġas Ġan Ġinitial Ġguess . ĊĠĠĠĠ Ġ* ĠThis Ġvalue Ġis Ġdetermined Ġonce Ġthe Ġcal ibr ator Ġstarts . ĊĠĠĠĠ Ġ* ĠIf Ġ[ sensor Type ] Ġis Ġ[ Accel erometer Sensor Collector . Sensor Type . AC CEL ERO METER _ UN CAL IB R ATED ], Ġthis ĊĠĠĠĠ Ġ* Ġwill Ġbe Ġequal Ġto Ġthe Ġvalues Ġused Ġinternally Ġby Ġthe Ġdevice Ġas Ġpart Ġof Ġthe Ġaccel erometer ĊĠĠĠĠ Ġ* Ġhardware Ġcalibration . ĊĠĠĠĠ Ġ* ĠIf Ġ[ sensor Type ] Ġis Ġ[ Accel erometer Sensor Collector . Sensor Type . AC CEL ERO METER ], Ġthen ĊĠĠĠĠ Ġ* Ġaccel erometer Ġsensor Ġmeasurements Ġare Ġassumed Ġto Ġbe Ġalready Ġbias Ġcomp ens ated , Ġand Ġthe Ġinitial ĊĠĠĠĠ Ġ* Ġbias Ġis Ġassumed Ġto Ġbe Ġzero . ĊĠĠĠĠ Ġ* ĠIf Ġ[ is Ground Truth Initial Bias ] Ġis Ġtrue , Ġthis Ġis Ġassumed Ġto Ġbe Ġthe Ġtrue Ġbias , Ġand Ġ[ estimated Bias As Tri ad ] ĊĠĠĠĠ Ġ* Ġwill Ġbe Ġequal Ġto Ġthis Ġvalue , Ġotherwise Ġ[ estimated Bias As Tri ad ] Ġwill Ġbe Ġthe Ġestimated Ġbias ĊĠĠĠĠ Ġ* Ġafter Ġsolving Ġcalibration , Ġwhich Ġwill Ġdiffer Ġfrom Ġ[ estimated Bias As Tri ad ]. ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġval Ġinitial Bias As Tri ad : ĠAccel eration Tri ad ? ĊĠĠĠĠĠĠĠ Ġget () Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġinitial Bias X Ġ= Ġthis . initial Bias X ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġinitial Bias Y Ġ= Ġthis . initial Bias Y ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġinitial Bias Z Ġ= Ġthis . initial Bias Z ĊĠĠĠĠĠĠĠĠĠĠĠ Ġreturn Ġif Ġ( initial Bias X Ġ!= Ġnull Ġ&& Ġinitial Bias Y Ġ!= Ġnull Ġ&& Ġinitial Bias Z Ġ!= Ġnull ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠAccel eration Tri ad ( ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠAccel eration Unit . METER S _ PER _ S QUARE D _ SECOND , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġinitial Bias X , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġinitial Bias Y , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġinitial Bias Z ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġnull ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġinitial Ġbias Ġcoordinates Ġused Ġas Ġan Ġinitial Ġguess . ĊĠĠĠĠ Ġ* ĠThis Ġvalue Ġis Ġdetermined Ġonce Ġthe Ġcal ibr ator Ġstarts . ĊĠĠĠĠ Ġ* ĠIf Ġ[ sensor Type ] Ġis Ġ[ Accel erometer Sensor Collector . Sensor Type . AC CEL ERO METER _ UN CAL IB R ATED ], Ġthis ĊĠĠĠĠ Ġ* Ġwill Ġbe Ġequal Ġto Ġthe Ġvalues Ġused Ġinternally Ġby Ġthe Ġdevice Ġas Ġpart Ġof Ġthe Ġaccel erometer ĊĠĠĠĠ Ġ* Ġhardware Ġcalibration . ĊĠĠĠĠ Ġ* ĠIf Ġ[ sensor Type ] Ġis Ġ[ Accel erometer Sensor Collector . Sensor Type . AC CEL ERO METER ], Ġthen ĊĠĠĠĠ Ġ* Ġaccel erometer Ġsensor Ġmeasurements Ġare Ġassumed Ġto Ġbe Ġalready Ġbias Ġcomp ens ated , Ġand Ġthe Ġinitial ĊĠĠĠĠ Ġ* Ġbias Ġis Ġassumed Ġto Ġbe Ġzero . ĊĠĠĠĠ Ġ* ĠIf Ġ[ is Ground Truth Initial Bias ] Ġis Ġtrue , Ġthis Ġis Ġassumed Ġto Ġbe Ġthe Ġtrue Ġbias , Ġand Ġ[ estimated Bias As Tri ad ] ĊĠĠĠĠ Ġ* Ġwill Ġbe Ġequal Ġto Ġthis Ġvalue , Ġotherwise Ġ[ estimated Bias As Tri ad ] Ġwill Ġbe Ġthe Ġestimated Ġbias ĊĠĠĠĠ Ġ* Ġafter Ġsolving Ġcalibration , Ġwhich Ġwill Ġdiffer Ġfrom Ġ[ estimated Bias As Tri ad ]. ĊĠĠĠĠ Ġ* ĊĠĠĠĠ Ġ* Ġ@ param Ġresult Ġinstance Ġwhere Ġresult Ġwill Ġbe Ġstored . ĊĠĠĠĠ Ġ* Ġ@ return Ġtrue Ġif Ġresult Ġis Ġavailable , Ġfalse Ġotherwise . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġget Initial Bias As Tri ad ( result : ĠAccel eration Tri ad ): ĠBoolean Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġinitial Bias X Ġ= Ġthis . initial Bias X ĊĠĠĠĠĠĠĠ Ġval Ġinitial Bias Y Ġ= Ġthis . initial Bias Y ĊĠĠĠĠĠĠĠ Ġval Ġinitial Bias Z Ġ= Ġthis . initial Bias Z ĊĠĠĠĠĠĠĠ Ġreturn Ġif Ġ( initial Bias X Ġ!= Ġnull Ġ&& Ġinitial Bias Y Ġ!= Ġnull Ġ&& Ġinitial Bias Z Ġ!= Ġnull ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġresult . setValue Coordinates And Unit ( ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġinitial Bias X , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġinitial Bias Y , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġinitial Bias Z , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠAccel eration Unit . METER S _ PER _ S QUARE D _ SECOND ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġtrue ĊĠĠĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġfalse ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠIndicates Ġwhether Ġinitial Ġbias Ġis Ġconsidered Ġa Ġground - truth Ġknown Ġbias . ĊĠĠĠĠ Ġ* ĠWhen Ġtrue , Ġestimated Ġbi ases Ġare Ġexactly Ġequal Ġto Ġinitial Ġbi ases , Ġotherwise ĊĠĠĠĠ Ġ* Ġinitial Ġbi ases Ġare Ġjust Ġan Ġinitial Ġguess Ġand Ġestimated Ġones Ġmight Ġdiffer Ġafter ĊĠĠĠĠ Ġ* Ġsolving Ġcalibration . ĊĠĠĠĠ Ġ* ĊĠĠĠĠ Ġ* Ġ@ throws ĠIllegalStateException Ġif Ġcal ibr ator Ġis Ġalready Ġrunning . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġvar Ġis Ground Truth Initial Bias : ĠBoolean Ġ= Ġfalse ĊĠĠĠĠĠĠĠ Ġ@ Throws ( IllegalStateException :: class ) ĊĠĠĠĠĠĠĠ Ġset ( value ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġcheck (! running ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġfield Ġ= Ġvalue ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠLocation Ġof Ġdevice Ġwhen Ġrunning Ġcalibration . ĊĠĠĠĠ Ġ* ĠIf Ġlocation Ġis Ġprovided , ĠW GS 8 4 ĠEarth Ġmodel Ġis Ġused Ġto Ġdetermine Ġgravity Ġnorm ĊĠĠĠĠ Ġ* Ġat Ġsuch Ġlocation , Ġotherwise Ġgravity Ġnorm Ġis Ġestimated Ġduring Ġinitialization Ġby Ġusing Ġthe ĊĠĠĠĠ Ġ* Ġgravity Ġsensor Ġof Ġdevice . ĊĠĠĠĠ Ġ* ĊĠĠĠĠ Ġ* Ġ@ throws ĠIllegalStateException Ġif Ġcal ibr ator Ġis Ġalready Ġrunning . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġvar Ġlocation : ĠLocation ? Ġ= Ġnull ĊĠĠĠĠĠĠĠ Ġ@ Throws ( IllegalStateException :: class ) ĊĠĠĠĠĠĠĠ Ġset ( value ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġcheck (! running ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġfield Ġ= Ġvalue ĊĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( value Ġ!= Ġnull ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġset Ġgravity Ġnorm Ġbased Ġon Ġprovided Ġlocation ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġgravity Norm Ġ= ĠGr avity Helper . get Gravity Norm For Location ( value ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠIndicates Ġwhether Ġgravity Ġnorm Ġis Ġestimated Ġduring Ġinitialization . ĊĠĠĠĠ Ġ* ĠIf Ġlocation Ġis Ġprovided , Ġgravity Ġis Ġnot Ġestimated Ġand Ġinstead Ġtheoret ical ĊĠĠĠĠ Ġ* Ġgravity Ġfor Ġprovided Ġlocation Ġis Ġused . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġval Ġis Gravity Norm Est imated ĊĠĠĠĠĠĠĠ Ġget () Ġ= Ġlocation Ġ== Ġnull ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġaccel erometer Ġsensor Ġbeing Ġused Ġfor Ġinterval Ġdetection . ĊĠĠĠĠ Ġ* ĠThis Ġcan Ġbe Ġused Ġto Ġobtain Ġadditional Ġinformation Ġabout Ġthe Ġsensor . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġval Ġaccel erometer Sensor ĊĠĠĠĠĠĠĠ Ġget () Ġ= Ġinterval Detector . sensor ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġgravity Ġsensor Ġbeing Ġused Ġfor Ġgravity Ġestimation . ĊĠĠĠĠ Ġ* ĠThis Ġcan Ġbe Ġused Ġto Ġobtain Ġadditional Ġinformation Ġabout Ġthe Ġsensor . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġval Ġgravity Sensor ĊĠĠĠĠĠĠĠ Ġget () Ġ= Ġgravity Norm Estimator . sensor ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġor Ġsets Ġinitial Ġscaling Ġfactors Ġand Ġcross Ġcoup ing Ġerrors Ġmatrix . ĊĠĠĠĠ Ġ* ĊĠĠĠĠ Ġ* Ġ@ throws ĠIllegalStateException Ġif Ġcal ibr ator Ġis Ġcurrently Ġrunning . ĊĠĠĠĠ Ġ* Ġ@ throws ĠIllegalArgumentException Ġif Ġprovided Ġmatrix Ġis Ġnot Ġ 3 x 3 . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġvar Ġinitial Ma : ĠMatrix ĊĠĠĠĠĠĠĠ Ġget () Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġresult Ġ= ĠMatrix ( Body Kin em atics . COMPONENTS , ĠBody Kin em atics . COMPONENTS ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġget Initial Ma ( result ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġreturn Ġresult ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġ@ Throws ( IllegalStateException :: class , ĠIllegalArgumentException :: class ) ĊĠĠĠĠĠĠĠ Ġset ( value ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġcheck (! running ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġrequire ( value . rows Ġ== ĠBody Kin em atics . COMPONENTS Ġ&& Ġvalue . columns Ġ== ĠBody Kin em atics . COMPONENTS ) ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġinitial Sx Ġ= Ġvalue . getElement AtIndex ( 0 ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġinitial My x Ġ= Ġvalue . getElement AtIndex ( 1 ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġinitial Mz x Ġ= Ġvalue . getElement AtIndex ( 2 ) ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġinitial M xy Ġ= Ġvalue . getElement AtIndex ( 3 ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġinitial Sy Ġ= Ġvalue . getElement AtIndex ( 4 ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġinitial M zy Ġ= Ġvalue . getElement AtIndex ( 5 ) ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġinitial M xz Ġ= Ġvalue . getElement AtIndex ( 6 ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġinitial My z Ġ= Ġvalue . getElement AtIndex ( 7 ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġinitial Sz Ġ= Ġvalue . getElement AtIndex ( 8 ) ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġinitial Ġscale Ġfactors Ġand Ġcross Ġcoup ling Ġerrors Ġmatrix . ĊĠĠĠĠ Ġ* ĊĠĠĠĠ Ġ* Ġ@ param Ġresult Ġinstance Ġwhere Ġdata Ġwill Ġbe Ġstored . ĊĠĠĠĠ Ġ* Ġ@ throws ĠIllegalArgumentException Ġif Ġprovided Ġresult Ġmatrix Ġis Ġnot Ġ 3 x 3 . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġ@ Throws ( IllegalArgumentException :: class ) ĊĠĠĠ Ġfun Ġget Initial Ma ( result : ĠMatrix ) Ġ{ ĊĠĠĠĠĠĠĠ Ġrequire ( result . rows Ġ== ĠBody Kin em atics . COMPONENTS Ġ&& Ġresult . columns Ġ== ĠBody Kin em atics . COMPONENTS ) ĊĊĠĠĠĠĠĠĠ Ġresult . set Element AtIndex ( 0 , Ġinitial Sx ) ĊĠĠĠĠĠĠĠ Ġresult . set Element AtIndex ( 1 , Ġinitial My x ) ĊĠĠĠĠĠĠĠ Ġresult . set Element AtIndex ( 2 , Ġinitial Mz x ) ĊĊĠĠĠĠĠĠĠ Ġresult . set Element AtIndex ( 3 , Ġinitial M xy ) ĊĠĠĠĠĠĠĠ Ġresult . set Element AtIndex ( 4 , Ġinitial Sy ) ĊĠĠĠĠĠĠĠ Ġresult . set Element AtIndex ( 5 , Ġinitial M zy ) ĊĊĠĠĠĠĠĠĠ Ġresult . set Element AtIndex ( 6 , Ġinitial M xz ) ĊĠĠĠĠĠĠĠ Ġresult . set Element AtIndex ( 7 , Ġinitial My z ) ĊĠĠĠĠĠĠĠ Ġresult . set Element AtIndex ( 8 , Ġinitial Sz ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġminimum Ġnumber Ġof Ġrequired Ġmeasurements Ġto Ġstart Ġcalibration . ĊĠĠĠĠ Ġ* ĠEach Ġtime Ġthat Ġthe Ġdevice Ġis Ġkept Ġstatic , Ġa Ġnew Ġmeasurement Ġis Ġcollected . ĊĠĠĠĠ Ġ* ĠWhen Ġthe Ġrequired Ġnumber Ġof Ġmeasurements Ġis Ġcollected , Ġcalibration Ġcan Ġstart . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġoverride Ġval Ġminimum Required Measure ments : ĠInt ĊĠĠĠĠĠĠĠ Ġget () Ġ= Ġif Ġ( is Common Axis Used ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( is Ground Truth Initial Bias ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠK NO WN _ BI AS _ MIN IMUM _ MEAS UREMENT S _ COMMON _ Z _ AXIS ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠUNKNOWN _ BI AS _ MIN IMUM _ MEAS UREMENT S _ COMMON _ Z _ AXIS ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( is Ground Truth Initial Bias ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠK NO WN _ BI AS _ MIN IMUM _ MEAS UREMENT S _ GENERAL ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠUNKNOWN _ BI AS _ MIN IMUM _ MEAS UREMENT S _ GENERAL ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġestimated Ġaverage Ġof Ġgravity Ġnorm Ġexpressed Ġin Ġmeters Ġper Ġsquared Ġsecond Ġ( m / s ^ 2 ). ĊĠĠĠĠ Ġ* ĠThis Ġis Ġonly Ġavailable Ġif Ġno Ġlocation Ġis Ġprovided Ġand Ġinitialization Ġhas Ġcompleted . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġval Ġaverage Gravity Norm ĊĠĠĠĠĠĠĠ Ġget () Ġ= Ġif Ġ( is Gravity Norm Est imated ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġgravity Norm Estimator . average Norm ĊĠĠĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġnull ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġestimated Ġaverage Ġgravity Ġnorm Ġas ĠAccel eration . ĊĠĠĠĠ Ġ* ĠThis Ġis Ġonly Ġavailable Ġif Ġno Ġlocation Ġis Ġprovided Ġand Ġinitialization Ġhas Ġcompleted . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġval Ġaverage Gravity Norm As Measurement ĊĠĠĠĠĠĠĠ Ġget () Ġ= Ġif Ġ( is Gravity Norm Est imated ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġgravity Norm Estimator . average Norm As Measurement ĊĠĠĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġnull ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġestimated Ġaverage Ġgravity Ġnorm Ġas ĠAccel eration . ĊĠĠĠĠ Ġ* ĠThis Ġis Ġonly Ġavailable Ġif Ġno Ġlocation Ġis Ġprovided Ġand Ġinitialization Ġhas Ġcompleted . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġget Average Gravity Norm As Measurement ( result : ĠAccel eration ): ĠBoolean Ġ{ ĊĠĠĠĠĠĠĠ Ġreturn Ġif Ġ( is Gravity Norm Est imated ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġgravity Norm Estimator . get Average Norm As Measurement ( result ) ĊĠĠĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġfalse ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġestimated Ġvariance Ġof Ġgravity Ġnorm Ġexpressed Ġin Ġ( m ^ 2 / s ^ 4 ). ĊĠĠĠĠ Ġ* ĠThis Ġis Ġonly Ġavailable Ġif Ġno Ġlocation Ġis Ġprovided Ġand Ġinitialization Ġhas Ġcompleted . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġval Ġgravity Norm Variance ĊĠĠĠĠĠĠĠ Ġget () Ġ= Ġif Ġ( is Gravity Norm Est imated ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġgravity Norm Estimator . norm Variance ĊĠĠĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġnull ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġestimated Ġstandard Ġdeviation Ġof Ġgravity Ġnorm Ġexpressed Ġin Ġmeters Ġper Ġsquared Ġsecond ĊĠĠĠĠ Ġ* Ġ( m / s ^ 2 ). ĊĠĠĠĠ Ġ* ĠThis Ġis Ġonly Ġavailable Ġif Ġno Ġlocation Ġis Ġprovided Ġand Ġinitialization Ġhas Ġcompleted . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġval Ġgravity Norm Standard De viation ĊĠĠĠĠĠĠĠ Ġget () Ġ= Ġif Ġ( is Gravity Norm Est imated ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġgravity Norm Estimator . norm Standard De viation ĊĠĠĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġnull ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġestimated Ġstandard Ġdeviation Ġof Ġgravity Ġnorm Ġas ĠAccel eration . ĊĠĠĠĠ Ġ* ĠThis Ġis Ġonly Ġavailable Ġif Ġno Ġlocation Ġis Ġprovided Ġand Ġinitialization Ġhas Ġcompleted . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġval Ġgravity Norm Standard De viation As Measurement ĊĠĠĠĠĠĠĠ Ġget () Ġ= Ġif Ġ( is Gravity Norm Est imated ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġgravity Norm Estimator . norm Standard De viation As Measurement ĊĠĠĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġnull ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġestimated Ġstandard Ġdeviation Ġof Ġgravity Ġnorm Ġas ĠAccel eration . ĊĠĠĠĠ Ġ* ĠThis Ġis Ġonly Ġavailable Ġif Ġno Ġlocation Ġis Ġprovided Ġand Ġinitialization Ġhas Ġcompleted . ĊĠĠĠĠ Ġ* ĊĠĠĠĠ Ġ* Ġ@ param Ġresult Ġinstance Ġwhere Ġresult Ġwill Ġbe Ġstored . ĊĠĠĠĠ Ġ* Ġ@ return Ġtrue Ġi Ġresult Ġis Ġavailable , Ġfalse Ġotherwise . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġget Gravity Norm Standard De viation As Measurement ( result : ĠAccel eration ): ĠBoolean Ġ{ ĊĠĠĠĠĠĠĠ Ġreturn Ġif Ġ( is Gravity Norm Est imated ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġgravity Norm Estimator . get Norm Standard De viation As Measurement ( result ) ĊĠĠĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġfalse ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets ĠP SD Ġ( Power ĠSpect ral ĠD ensity ) Ġof Ġgravity Ġnorm Ġexpressed Ġin Ġ( m ^ 2 Ġ* Ġs ^ - 3 ). ĊĠĠĠĠ Ġ* ĠThis Ġis Ġonly Ġavailable Ġif Ġno Ġlocation Ġis Ġprovided Ġand Ġinitialization Ġhas Ġcompleted . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġval Ġgravity P sd ĊĠĠĠĠĠĠĠ Ġget () Ġ= Ġif Ġ( is Gravity Norm Est imated ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġgravity Norm Estimator . ps d ĊĠĠĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġnull ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġroot ĠP SD Ġ( Power ĠSpect ral ĠD ensity ) Ġof Ġgravity Ġnorm Ġexpressed Ġin Ġ( m Ġ* Ġs ^ - 1 . 5 ). ĊĠĠĠĠ Ġ* ĠThis Ġis Ġonly Ġavailable Ġif Ġno Ġlocation Ġis Ġprovided Ġand Ġinitialization Ġhas Ġcompleted . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġval Ġgravity Root P sd ĊĠĠĠĠĠĠĠ Ġget () Ġ= Ġif Ġ( is Gravity Norm Est imated ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġgravity Norm Estimator . root P sd ĊĠĠĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġnull ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġestimated Ġaccel erometer Ġscale Ġfactors Ġand Ġcross Ġcoup ling Ġerrors , Ġor Ġnull Ġif Ġnot ĊĠĠĠĠ Ġ* Ġavailable . ĊĠĠĠĠ Ġ* ĠThis Ġis Ġthe Ġproduct Ġof Ġmatrix ĠTa Ġcontaining Ġcross Ġcoup ling Ġerrors Ġand ĠK a ĊĠĠĠĠ Ġ* Ġcontaining Ġscaling Ġfactors . ĊĠĠĠĠ Ġ* ĠSo Ġt at : ĊĠĠĠĠ Ġ* Ġ< pre > ĊĠĠĠĠ Ġ* ĠĠĠĠ ĠMa Ġ= Ġ[ sx ĠĠĠ Ġm xy Ġ Ġmx z ] Ġ= ĠTa * Ka ĊĠĠĠĠ Ġ* ĠĠĠĠĠĠĠĠĠ Ġ[ my x ĠĠ Ġsy ĠĠ Ġmy z ] ĊĠĠĠĠ Ġ* ĠĠĠĠĠĠĠĠĠ Ġ[ m zx ĠĠ Ġm zy Ġ Ġsz Ġ] ĊĠĠĠĠ Ġ* Ġ</ pre > ĊĠĠĠĠ Ġ* ĠWhere : ĊĠĠĠĠ Ġ* Ġ< pre > ĊĠĠĠĠ Ġ* ĠĠĠĠ ĠK a Ġ= Ġ[ sx Ġ 0 ĠĠĠ 0 Ġ] ĊĠĠĠĠ Ġ* ĠĠĠĠĠĠĠĠĠ Ġ[ 0 Ġ Ġsy ĠĠ 0 Ġ] ĊĠĠĠĠ Ġ* ĠĠĠĠĠĠĠĠĠ Ġ[ 0 ĠĠ 0 ĠĠ Ġsz ] ĊĠĠĠĠ Ġ* Ġ</ pre > ĊĠĠĠĠ Ġ* Ġand ĊĠĠĠĠ Ġ* Ġ< pre > ĊĠĠĠĠ Ġ* ĠĠĠĠ ĠTa Ġ= Ġ[ 1 ĠĠĠĠĠĠĠĠĠ Ġ- alpha Xy ĠĠĠ Ġalpha Xz Ġ] ĊĠĠĠĠ Ġ* ĠĠĠĠĠĠĠĠĠ Ġ[ alpha Yx ĠĠĠĠ 1 ĠĠĠĠĠĠĠĠĠĠ Ġ- alpha Yz ] ĊĠĠĠĠ Ġ* ĠĠĠĠĠĠĠĠĠ Ġ[- alpha Zx ĠĠ Ġalpha Zy ĠĠĠĠĠ 1 ĠĠĠĠĠĠ Ġ] ĊĠĠĠĠ Ġ* Ġ</ pre > ĊĠĠĠĠ Ġ* ĠHence : ĊĠĠĠĠ Ġ* Ġ< pre > ĊĠĠĠĠ Ġ* ĠĠĠĠ ĠMa Ġ= Ġ[ sx ĠĠĠ Ġm xy Ġ Ġmx z ] Ġ= ĠTa * Ka Ġ= Ġ Ġ[ sx ĠĠĠĠĠĠĠĠĠĠĠĠ Ġ- sy Ġ* Ġalpha Xy ĠĠ Ġsz Ġ* Ġalpha Xz Ġ] ĊĠĠĠĠ Ġ* ĠĠĠĠĠĠĠĠĠ Ġ[ my x ĠĠ Ġsy ĠĠ Ġmy z ] ĠĠĠĠĠĠĠĠĠĠĠ Ġ[ sx Ġ* Ġalpha Yx ĠĠ Ġsy ĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ- sz Ġ* Ġalpha Yz ] ĊĠĠĠĠ Ġ* ĠĠĠĠĠĠĠĠĠ Ġ[ m zx ĠĠ Ġm zy Ġ Ġsz Ġ] ĠĠĠĠĠĠĠĠĠĠĠ Ġ[- sx Ġ* Ġalpha Zx Ġ Ġsy Ġ* Ġalpha Zy ĠĠĠ Ġsz ĠĠĠĠĠĠĠĠĠĠ Ġ] ĊĠĠĠĠ Ġ* Ġ</ pre > ĊĠĠĠĠ Ġ* ĠThis Ġinstance Ġallows Ġany Ġ 3 x 3 Ġmatrix Ġhowever , Ġtypically Ġalpha Yx , Ġalpha Zx Ġand Ġalpha Zy ĊĠĠĠĠ Ġ* Ġare Ġconsidered Ġto Ġbe Ġzero Ġif Ġthe Ġaccel erometer Ġz - axis Ġis Ġassumed Ġto Ġbe Ġthe Ġsame ĊĠĠĠĠ Ġ* Ġas Ġthe Ġbody Ġz - axis . ĠWhen Ġthis Ġis Ġassumed , Ġmy x Ġ= Ġm zx Ġ= Ġm zy Ġ= Ġ 0 Ġand Ġthe ĠMa Ġmatrix ĊĠĠĠĠ Ġ* Ġbecomes Ġupper Ġdiagonal : ĊĠĠĠĠ Ġ* Ġ< pre > ĊĠĠĠĠ Ġ* ĠĠĠĠ ĠMa Ġ= Ġ[ sx ĠĠĠ Ġm xy Ġ Ġmx z ] ĊĠĠĠĠ Ġ* ĠĠĠĠĠĠĠĠĠ Ġ[ 0 ĠĠĠĠ Ġsy ĠĠ Ġmy z ] ĊĠĠĠĠ Ġ* ĠĠĠĠĠĠĠĠĠ Ġ[ 0 ĠĠĠĠĠ 0 ĠĠĠ Ġsz Ġ] ĊĠĠĠĠ Ġ* Ġ</ pre > ĊĠĠĠĠ Ġ* ĠValues Ġof Ġthis Ġmatrix Ġare Ġunit - less . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġval Ġestimated Ma ĊĠĠĠĠĠĠĠ Ġget () Ġ= Ġinternal Cal ibr ator ?. estimated Ma ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġestimated Ġx - axis Ġscale Ġfactor Ġor Ġnull Ġif Ġnot Ġavailable . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġoverride Ġval Ġestimated Sx ĊĠĠĠĠĠĠĠ Ġget () Ġ= Ġinternal Cal ibr ator ?. estimated Sx ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġestimated Ġy - axis Ġscale Ġfactor Ġor Ġnull Ġif Ġnot Ġavailable . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġoverride Ġval Ġestimated Sy ĊĠĠĠĠĠĠĠ Ġget () Ġ= Ġinternal Cal ibr ator ?. estimated Sy ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġestimated Ġz - axis Ġscale Ġfactor Ġor Ġnull Ġif Ġnot Ġavailable . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġoverride Ġval Ġestimated Sz ĊĠĠĠĠĠĠĠ Ġget () Ġ= Ġinternal Cal ibr ator ?. estimated Sz ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġestimated Ġx - y Ġcross - coup ling Ġerror Ġor Ġnull Ġif Ġnot Ġavailable . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġoverride Ġval Ġestimated M xy ĊĠĠĠĠĠĠĠ Ġget () Ġ= Ġinternal Cal ibr ator ?. estimated M xy ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġestimated Ġx - z Ġcross - coup ling Ġerror Ġor Ġnull Ġif Ġnot Ġavailable . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġoverride Ġval Ġestimated M xz ĊĠĠĠĠĠĠĠ Ġget () Ġ= Ġinternal Cal ibr ator ?. estimated M xz ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġestimated Ġy - x Ġcross - coup ling Ġerror Ġor Ġnull Ġif Ġnot Ġavailable . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġoverride Ġval Ġestimated My x ĊĠĠĠĠĠĠĠ Ġget () Ġ= Ġinternal Cal ibr ator ?. estimated My x ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġestimated Ġy - z Ġcross - coup ling Ġerror Ġor Ġnull Ġif Ġnot Ġavailable . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġoverride Ġval Ġestimated My z ĊĠĠĠĠĠĠĠ Ġget () Ġ= Ġinternal Cal ibr ator ?. estimated My z ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġestimated Ġz - x Ġcross - coup ling Ġerror Ġor Ġnull Ġif Ġnot Ġavailable . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġoverride Ġval Ġestimated Mz x ĊĠĠĠĠĠĠĠ Ġget () Ġ= Ġinternal Cal ibr ator ?. estimated Mz x ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġestimated Ġz - y Ġcross - coup ling Ġerror Ġor Ġnull Ġif Ġnot Ġavailable . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġoverride Ġval Ġestimated M zy ĊĠĠĠĠĠĠĠ Ġget () Ġ= Ġinternal Cal ibr ator ?. estimated M zy ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġestimated Ġcovariance Ġmatrix Ġfor Ġestimated Ġaccel erometer Ġparameters Ġor Ġnull Ġif Ġnot ĊĠĠĠĠ Ġ* Ġavailable . ĊĠĠĠĠ Ġ* ĠWhen Ġbias Ġis Ġknown , Ġdiagonal Ġelements Ġof Ġthe Ġcovariance Ġmatrix Ġcontains Ġvariance Ġfor Ġthe ĊĠĠĠĠ Ġ* Ġfollowing Ġparameters Ġ( following Ġindicated Ġorder ): Ġsx , Ġsy , Ġsz , Ġm xy , Ġmx z , Ġmy z , Ġm zx , Ġm zy . ĊĠĠĠĠ Ġ* ĠWhen Ġbias Ġis Ġnot Ġknown , Ġdiagonal Ġelements Ġof Ġthe Ġcovariance Ġmatrix Ġcontains Ġvariance Ġfor ĊĠĠĠĠ Ġ* Ġthe Ġfollowing Ġparameters Ġ( following Ġindicated Ġorder ): Ġbx , Ġby , Ġb z , Ġsx , Ġsy , Ġsz , Ġm xy , Ġmx z , ĊĠĠĠĠ Ġ* Ġmy x , Ġmy z , Ġm zx , Ġm zy . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġoverride Ġval Ġestimated Co variance ĊĠĠĠĠĠĠĠ Ġget () Ġ= Ġinternal Cal ibr ator ?. estimated Co variance ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġestimated Ġchi Ġsquare Ġvalue Ġor Ġnull Ġif Ġnot Ġavailable . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġoverride Ġval Ġestimated Chi Sq ĊĠĠĠĠĠĠĠ Ġget () Ġ= Ġinternal Cal ibr ator ?. estimated Chi Sq ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġestimated Ġmean Ġsquare Ġerror Ġrespect Ġto Ġprovided Ġmeasurements Ġor Ġnull Ġif Ġnot Ġavailable . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġoverride Ġval Ġestimated M se ĊĠĠĠĠĠĠĠ Ġget () Ġ= Ġinternal Cal ibr ator ?. estimated M se ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġx Ġcoordinate Ġof Ġestimated Ġaccel erometer Ġbias Ġexpressed Ġin Ġmeters Ġper Ġsquared Ġsecond ĊĠĠĠĠ Ġ* Ġ( m / s ^ 2 ). ĊĠĠĠĠ Ġ* ĠIf Ġ[ is Ground Truth Initial Bias ] Ġis Ġtrue , Ġthis Ġwill Ġbe Ġequal Ġto Ġ[ initial Bias X ], Ġotherwise Ġit ĊĠĠĠĠ Ġ* Ġwill Ġbe Ġthe Ġestimated Ġvalue Ġobtained Ġafter Ġsolving Ġcalibration , Ġthat Ġmight Ġdiffer Ġfrom ĊĠĠĠĠ Ġ* Ġ[ initial Bias X ]. ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġval Ġestimated Bias X : ĠDouble ? ĊĠĠĠĠĠĠĠ Ġget () Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġreturn Ġwhen Ġ( val Ġinternal Cal ibr ator Ġ= Ġthis . internal Cal ibr ator ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġis ĠUnknown Bias Accel erometer Cal ibr ator Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġinternal Cal ibr ator . estimated Bias Fx ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġis ĠKnown Bias Accel erometer Cal ibr ator Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġinternal Cal ibr ator . bias X ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġelse Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġnull ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġy Ġcoordinate Ġof Ġestimated Ġaccel erometer Ġbias Ġexpressed Ġin Ġmeters Ġper Ġsquared Ġsecond ĊĠĠĠĠ Ġ* Ġ( m / s ^ 2 ). ĊĠĠĠĠ Ġ* ĠIf Ġ[ is Ground Truth Initial Bias ] Ġis Ġtrue , Ġthis Ġwill Ġbe Ġequal Ġto Ġ[ initial Bias Y ], Ġotherwise Ġit ĊĠĠĠĠ Ġ* Ġwill Ġbe Ġthe Ġestimated Ġvalue Ġobtained Ġafter Ġsolving Ġcalibration , Ġthat Ġmight Ġdiffer Ġfrom ĊĠĠĠĠ Ġ* Ġ[ initial Bias Y ]. ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġval Ġestimated Bias Y : ĠDouble ? ĊĠĠĠĠĠĠĠ Ġget () Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġreturn Ġwhen Ġ( val Ġinternal Cal ibr ator Ġ= Ġthis . internal Cal ibr ator ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġis ĠUnknown Bias Accel erometer Cal ibr ator Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġinternal Cal ibr ator . estimated Bias Fy ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġis ĠKnown Bias Accel erometer Cal ibr ator Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġinternal Cal ibr ator . bias Y ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġelse Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġnull ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġz Ġcoordinate Ġof Ġestimated Ġaccel erometer Ġbias Ġexpressed Ġin Ġmeters Ġper Ġsquared Ġsecond ĊĠĠĠĠ Ġ* Ġ( m / s ^ 2 ). ĊĠĠĠĠ Ġ* ĠIf Ġ[ is Ground Truth Initial Bias ] Ġis Ġtrue , Ġthis Ġwill Ġbe Ġequal Ġto Ġ[ initial Bias Z ], Ġotherwise Ġit ĊĠĠĠĠ Ġ* Ġwill Ġbe Ġthe Ġestimated Ġvalue Ġobtained Ġafter Ġsolving Ġcalibration , Ġthat Ġmight Ġdiffer Ġfrom ĊĠĠĠĠ Ġ* Ġ[ initial Bias Z ]. ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġval Ġestimated Bias Z : ĠDouble ? ĊĠĠĠĠĠĠĠ Ġget () Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġreturn Ġwhen Ġ( val Ġinternal Cal ibr ator Ġ= Ġthis . internal Cal ibr ator ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġis ĠUnknown Bias Accel erometer Cal ibr ator Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġinternal Cal ibr ator . estimated Bias Fz ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġis ĠKnown Bias Accel erometer Cal ibr ator Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġinternal Cal ibr ator . bias Z ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġelse Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġnull ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġx Ġcoordinate Ġof Ġestimated Ġaccel erometer Ġbias . ĊĠĠĠĠ Ġ* ĠIf Ġ[ is Ground Truth Initial Bias ] Ġis Ġtrue , Ġthis Ġwill Ġbe Ġequal Ġto Ġ[ initial Bias X As Measurement ], ĊĠĠĠĠ Ġ* Ġotherwise Ġit Ġwill Ġbe Ġthe Ġestimated Ġvalue Ġobtained Ġafter Ġsolving Ġcalibration , Ġthat Ġmight ĊĠĠĠĠ Ġ* Ġdiffer Ġfrom Ġ[ initial Bias X As Measurement ]. ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġval Ġestimated Bias X As Measurement : ĠAccel eration ? ĊĠĠĠĠĠĠĠ Ġget () Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġreturn Ġwhen Ġ( val Ġinternal Cal ibr ator Ġ= Ġthis . internal Cal ibr ator ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġis ĠUnknown Bias Accel erometer Cal ibr ator Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġinternal Cal ibr ator . estimated Bias Fx As Acceleration ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġis ĠKnown Bias Accel erometer Cal ibr ator Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġinternal Cal ibr ator . bias X As Acceleration ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġelse Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġnull ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġx Ġcoordinate Ġof Ġestimated Ġaccel erometer Ġbias . ĊĠĠĠĠ Ġ* ĠIf Ġ[ is Ground Truth Initial Bias ] Ġis Ġtrue , Ġresult Ġwill Ġbe Ġequal Ġto Ġ[ initial Bias X As Measurement ], ĊĠĠĠĠ Ġ* Ġotherwise Ġit Ġwill Ġbe Ġthe Ġestimated Ġvalue Ġobtained Ġafter Ġsolving Ġcalibration , Ġthat Ġmight ĊĠĠĠĠ Ġ* Ġdiffer Ġfrom Ġ[ initial Bias X As Measurement ]. ĊĠĠĠĠ Ġ* ĊĠĠĠĠ Ġ* Ġ@ param Ġresult Ġinstance Ġwhere Ġresult Ġwill Ġbe Ġstored . ĊĠĠĠĠ Ġ* Ġ@ return Ġtrue Ġif Ġresult Ġis Ġavailable , Ġfalse Ġotherwise . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġget Est imated Bias X As Measurement ( result : ĠAccel eration ): ĠBoolean Ġ{ ĊĠĠĠĠĠĠĠ Ġreturn Ġwhen Ġ( val Ġinternal Cal ibr ator Ġ= Ġthis . internal Cal ibr ator ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġis ĠUnknown Bias Accel erometer Cal ibr ator Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġinternal Cal ibr ator . get Est imated Bias Fx As Acceleration ( result ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġis ĠKnown Bias Accel erometer Cal ibr ator Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġinternal Cal ibr ator . get Bias X As Acceleration ( result ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtrue ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġelse Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġfalse ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġy Ġcoordinate Ġof Ġestimated Ġaccel erometer Ġbias . ĊĠĠĠĠ Ġ* ĠIf Ġ[ is Ground Truth Initial Bias ] Ġis Ġtrue , Ġthis Ġwill Ġbe Ġequal Ġto Ġ[ initial Bias Y As Measurement ], ĊĠĠĠĠ Ġ* Ġotherwise Ġit Ġwill Ġbe Ġthe Ġestimated Ġvalue Ġobtained Ġafter Ġsolving Ġcalibration , Ġthat Ġmight ĊĠĠĠĠ Ġ* Ġdiffer Ġfrom Ġ[ initial Bias Y As Measurement ]. ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġval Ġestimated Bias Y As Measurement : ĠAccel eration ? ĊĠĠĠĠĠĠĠ Ġget () Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġreturn Ġwhen Ġ( val Ġinternal Cal ibr ator Ġ= Ġthis . internal Cal ibr ator ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġis ĠUnknown Bias Accel erometer Cal ibr ator Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġinternal Cal ibr ator . estimated Bias Fy As Acceleration ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġis ĠKnown Bias Accel erometer Cal ibr ator Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġinternal Cal ibr ator . bias Y As Acceleration ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġelse Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġnull ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġy Ġcoordinate Ġof Ġestimated Ġaccel erometer Ġbias . ĊĠĠĠĠ Ġ* ĠIf Ġ[ is Ground Truth Initial Bias ] Ġis Ġtrue , Ġresult Ġwill Ġbe Ġequal Ġto Ġ[ initial Bias Y As Measurement ], ĊĠĠĠĠ Ġ* Ġotherwise Ġit Ġwill Ġbe Ġthe Ġestimated Ġvalue Ġobtained Ġafter Ġsolving Ġcalibration , Ġthat Ġmight ĊĠĠĠĠ Ġ* Ġdiffer Ġfrom Ġ[ initial Bias Y As Measurement ]. ĊĠĠĠĠ Ġ* ĊĠĠĠĠ Ġ* Ġ@ param Ġresult Ġinstance Ġwhere Ġresult Ġwill Ġbe Ġstored . ĊĠĠĠĠ Ġ* Ġ@ return Ġtrue Ġif Ġresult Ġis Ġavailable , Ġfalse Ġotherwise . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġget Est imated Bias Y As Measurement ( result : ĠAccel eration ): ĠBoolean Ġ{ ĊĠĠĠĠĠĠĠ Ġreturn Ġwhen Ġ( val Ġinternal Cal ibr ator Ġ= Ġthis . internal Cal ibr ator ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġis ĠUnknown Bias Accel erometer Cal ibr ator Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġinternal Cal ibr ator . get Est imated Bias Fy As Acceleration ( result ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġis ĠKnown Bias Accel erometer Cal ibr ator Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġinternal Cal ibr ator . get Bias Y As Acceleration ( result ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtrue ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġelse Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġfalse ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġz Ġcoordinate Ġof Ġestimated Ġaccel erometer Ġbias . ĊĠĠĠĠ Ġ* ĠIf Ġ[ is Ground Truth Initial Bias ] Ġis Ġtrue , Ġthis Ġwill Ġbe Ġequal Ġto Ġ[ initial Bias Z As Measurement ], ĊĠĠĠĠ Ġ* Ġotherwise Ġit Ġwill Ġbe Ġthe Ġestimated Ġvalue Ġobtained Ġafter Ġsolving Ġcalibration , Ġthat Ġmight ĊĠĠĠĠ Ġ* Ġdiffer Ġfrom Ġ[ initial Bias Z As Measurement ]. ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġval Ġestimated Bias Z As Measurement : ĠAccel eration ? ĊĠĠĠĠĠĠĠ Ġget () Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġreturn Ġwhen Ġ( val Ġinternal Cal ibr ator Ġ= Ġthis . internal Cal ibr ator ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġis ĠUnknown Bias Accel erometer Cal ibr ator Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġinternal Cal ibr ator . estimated Bias Fz As Acceleration ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġis ĠKnown Bias Accel erometer Cal ibr ator Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġinternal Cal ibr ator . bias Z As Acceleration ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġelse Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġnull ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġz Ġcoordinate Ġof Ġestimated Ġaccel erometer Ġbias . ĊĠĠĠĠ Ġ* ĠIf Ġ[ is Ground Truth Initial Bias ] Ġis Ġtrue , Ġresult Ġwill Ġbe Ġequal Ġto Ġ[ initial Bias Z As Measurement ], ĊĠĠĠĠ Ġ* Ġotherwise Ġit Ġwill Ġbe Ġthe Ġestimated Ġvalue Ġobtained Ġafter Ġsolving Ġcalibration , Ġthat Ġmight ĊĠĠĠĠ Ġ* Ġdiffer Ġfrom Ġ[ initial Bias Z As Measurement ]. ĊĠĠĠĠ Ġ* ĊĠĠĠĠ Ġ* Ġ@ param Ġresult Ġinstance Ġwhere Ġresult Ġwill Ġbe Ġstored . ĊĠĠĠĠ Ġ* Ġ@ return Ġtrue Ġif Ġresult Ġis Ġavailable , Ġfalse Ġotherwise . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġget Est imated Bias Z As Measurement ( result : ĠAccel eration ): ĠBoolean Ġ{ ĊĠĠĠĠĠĠĠ Ġreturn Ġwhen Ġ( val Ġinternal Cal ibr ator Ġ= Ġthis . internal Cal ibr ator ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġis ĠUnknown Bias Accel erometer Cal ibr ator Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġinternal Cal ibr ator . get Est imated Bias Fz As Acceleration ( result ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġis ĠKnown Bias Accel erometer Cal ibr ator Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġinternal Cal ibr ator . get Bias Z As Acceleration ( result ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtrue ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġelse Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġfalse ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġestimated Ġaccel erometer Ġbias . ĊĠĠĠĠ Ġ* ĠIf Ġ[ is Ground Truth Initial Bias ] Ġis Ġtrue , Ġthis Ġwill Ġbe Ġequal Ġto Ġ[ initial Bias As Tri ad ], ĊĠĠĠĠ Ġ* Ġotherwise Ġit Ġwill Ġbe Ġthe Ġestimated Ġvalue Ġobtained Ġafter Ġsolving Ġcalibration , Ġthat Ġmight ĊĠĠĠĠ Ġ* Ġdiffer Ġfrom Ġ[ initial Bias As Tri ad ]. ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġval Ġestimated Bias As Tri ad : ĠAccel eration Tri ad ? ĊĠĠĠĠĠĠĠ Ġget () Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġreturn Ġwhen Ġ( val Ġinternal Cal ibr ator Ġ= Ġthis . internal Cal ibr ator ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġis ĠUnknown Bias Accel erometer Cal ibr ator Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġinternal Cal ibr ator . estimated Bias As Tri ad ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġis ĠKnown Bias Accel erometer Cal ibr ator Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġinternal Cal ibr ator . bias As Tri ad ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġelse Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġnull ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġestimated Ġaccel erometer Ġbias . ĊĠĠĠĠ Ġ* ĠIf Ġ[ is Ground Truth Initial Bias ] Ġis Ġtrue , Ġresult Ġwill Ġbe Ġequal Ġto Ġ[ initial Bias As Tri ad ], ĊĠĠĠĠ Ġ* Ġotherwise Ġit Ġwill Ġbe Ġthe Ġestimated Ġvalue Ġobtained Ġafter Ġsolving Ġcalibration , Ġthat Ġmight ĊĠĠĠĠ Ġ* Ġdiffer Ġfrom Ġ[ initial Bias As Tri ad ]. ĊĠĠĠĠ Ġ* ĊĠĠĠĠ Ġ* Ġ@ param Ġresult Ġinstance Ġwhere Ġresult Ġwill Ġbe Ġstored . ĊĠĠĠĠ Ġ* Ġ@ return Ġtrue Ġif Ġresult Ġis Ġavailable , Ġfalse Ġotherwise . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġget Est imated Bias As Tri ad ( result : ĠAccel eration Tri ad ): ĠBoolean Ġ{ ĊĠĠĠĠĠĠĠ Ġreturn Ġwhen Ġ( val Ġinternal Cal ibr ator Ġ= Ġthis . internal Cal ibr ator ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġis ĠUnknown Bias Accel erometer Cal ibr ator Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġinternal Cal ibr ator . get Est imated Bias As Tri ad ( result ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġis ĠKnown Bias Accel erometer Cal ibr ator Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġinternal Cal ibr ator . get Bias As Tri ad ( result ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtrue ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġelse Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġfalse ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠGets Ġnorm Ġof Ġestimated Ġstandard Ġdeviation Ġof Ġaccel erometer Ġbias Ġexpressed Ġin Ġmeters Ġper ĊĠĠĠĠ Ġ* Ġsquared Ġsecond Ġ( m / s ^ 2 ), Ġor Ġnull Ġif Ġnot Ġavailable . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġval Ġestimated Bias Standard De viation Norm : ĠDouble ? ĊĠĠĠĠĠĠĠ Ġget () Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġinternal Cal ibr ator Ġ= Ġthis . internal Cal ibr ator Ġ?: Ġreturn Ġnull ĊĠĠĠĠĠĠĠĠĠĠĠ Ġreturn Ġif Ġ( internal Cal ibr ator Ġis ĠAcc eler ometer Bias Un cert ainty Source ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġinternal Cal ibr ator . estimated Bias Standard De viation Norm ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġnull ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠStarts Ġcal ibr ator . ĊĠĠĠĠ Ġ* ĠThis Ġmethod Ġstarts Ġcollecting Ġaccel erometer Ġmeasurements . ĊĠĠĠĠ Ġ* ĠWhen Ġcal ibr ator Ġis Ġstarted , Ġit Ġbegins Ġwith Ġan Ġinitialization Ġstage Ġwhere Ġaccel erometer Ġnoise ĊĠĠĠĠ Ġ* Ġis Ġestimated Ġwhile Ġdevice Ġremains Ġstatic . ĠIf Ġno Ġlocation Ġis Ġprovided , Ġduring Ġinitialization ĊĠĠĠĠ Ġ* Ġgravity Ġnorm Ġis Ġalso Ġestimated . ĊĠĠĠĠ Ġ* ĠOnce Ġinitialization Ġis Ġcompleted , Ġcal ibr ator Ġdetermines Ġintervals Ġwhere Ġdevice Ġremains Ġstatic ĊĠĠĠĠ Ġ* Ġwhen Ġdevice Ġhas Ġdifferent Ġpo ses , Ġso Ġthat Ġmeasurements Ġare Ġcollected Ġto Ġsolve Ġcalibration . ĊĠĠĠĠ Ġ* ĠIf Ġ[ solve Calibration When En ough Measure ments ] Ġis Ġtrue , Ġcalibration Ġis Ġautomatically Ġsolved ĊĠĠĠĠ Ġ* Ġonce Ġenough Ġmeasurements Ġare Ġcollected , Ġotherwise Ġa Ġcall Ġto Ġ[ cal ibrate ] Ġmust Ġbe Ġdone Ġto Ġsolve ĊĠĠĠĠ Ġ* Ġcalibration . ĊĠĠĠĠ Ġ* ĊĠĠĠĠ Ġ* Ġ@ throws ĠIllegalStateException Ġif Ġcal ibr ator Ġis Ġalready Ġrunning Ġor Ġa Ġsensor Ġis Ġmissing ĊĠĠĠĠ Ġ* Ġ( either Ġaccel erometer Ġor Ġgravity Ġif Ġit Ġis Ġbeing Ġused Ġwhen Ġno Ġlocation Ġis Ġprovided ). ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġ@ Throws ( IllegalStateException :: class ) ĊĠĠĠ Ġoverride Ġfun Ġstart () Ġ{ ĊĠĠĠĠĠĠĠ Ġsuper . start () ĊĠĠĠĠĠĠĠ Ġif Ġ( is Gravity Norm Est imated ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġgravity Norm Estimator . start () ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠSto ps Ġcal ibr ator . ĊĠĠĠĠ Ġ* ĠWhen Ġthis Ġis Ġcalled , Ġno Ġmore Ġaccel erometer Ġor Ġgravity Ġmeasurements Ġare Ġcollected . ĊĠĠĠĠ Ġ* ĊĠĠĠĠ Ġ* Ġ@ param Ġrunning Ġspecifies Ġthe Ġrunning Ġparameter Ġto Ġbe Ġset . ĠThis Ġis Ġtrue Ġwhen Ġstop Ġoccurs ĊĠĠĠĠ Ġ* Ġinternally Ġduring Ġmeasurement Ġcollection Ġto Ġstart Ġsolving Ġcalibration , Ġotherwise Ġis Ġfalse ĊĠĠĠĠ Ġ* Ġwhen Ġcalling Ġpublic Ġ[ stop ] Ġmethod . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġoverride Ġfun Ġinternal Stop ( running : ĠBoolean ) Ġ{ ĊĠĠĠĠĠĠĠ Ġif Ġ( gravity Norm Estimator . running ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġgravity Norm Estimator . stop () ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġsuper . internal Stop ( running ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠIntern ally Ġsolves Ġcalibration Ġusing Ġcollected Ġmeasurements Ġwithout Ġchecking Ġpre - requis ites ĊĠĠĠĠ Ġ* Ġ( if Ġeither Ġcal ibr ator Ġis Ġalready Ġrunning Ġor Ġenough Ġmeasurements Ġare Ġavailable ). ĊĠĠĠĠ Ġ* ĠThis Ġis Ġcalled Ġwhen Ġcalibration Ġoccurs Ġautomatically Ġwhen Ġenough Ġmeasurements Ġare Ġcollected . ĊĠĠĠĠ Ġ* ĠWhen Ġcalling Ġ[ cal ibrate ] Ġmethod , Ġpre - requis ites Ġare Ġchecked Ġbefore Ġcalling Ġthis Ġmethod . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġ@ Throws ( IllegalStateException :: class ) ĊĠĠĠ Ġoverride Ġfun Ġinternal Cal ibrate (): ĠBoolean Ġ{ ĊĠĠĠĠĠĠĠ Ġreturn Ġtry Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġcalibration Sol ving Started Listener ?. on Calibration Sol ving Started ( this ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġinternal Cal ibr ator ?. cal ibrate () ĊĠĠĠĠĠĠĠĠĠĠĠ Ġcalibration Completed Listener ?. on Calibration Completed ( this ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġrunning Ġ= Ġfalse ĊĠĠĠĠĠĠĠĠĠĠĠ Ġtrue ĊĠĠĠĠĠĠĠ Ġ} Ġcatch Ġ( e : ĠNavigation Exception ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ ĠLog . e ( ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠSingle Sensor Static Interval Accel erometer Cal ibr ator :: class . qualified Name , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ" Calibration Ġestimation Ġfailed ", ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġe ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġerror Listener ?. onError ( ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġthis , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠCal ibr ator Error Reason . NUM ER ICAL _ INST ABILITY _ D UR ING _ CAL IB RATION ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġrunning Ġ= Ġfalse ĊĠĠĠĠĠĠĠĠĠĠĠ Ġfalse ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠRe sets Ġcal ibr ator Ġto Ġits Ġinitial Ġstate . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġoverride Ġfun Ġreset () Ġ{ ĊĠĠĠĠĠĠĠ Ġsuper . reset () ĊĠĠĠĠĠĠĠ Ġgravity Norm Ġ= Ġnull ĊĠĠĠĠĠĠĠ Ġresult Un reli able Ġ= Ġfalse ĊĠĠĠĠĠĠĠ Ġinitial Bias X Ġ= Ġnull ĊĠĠĠĠĠĠĠ Ġinitial Bias Y Ġ= Ġnull ĊĠĠĠĠĠĠĠ Ġinitial Bias Z Ġ= Ġnull ĊĊĠĠĠĠĠĠĠ Ġinternal Cal ibr ator Ġ= Ġnull ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠUpdates Ġinitial Ġbi ases Ġvalues Ġwhen Ġfirst Ġaccel erometer Ġmeasurement Ġis Ġreceived , Ġso ĊĠĠĠĠ Ġ* Ġthat Ġhardware Ġcal ibr ated Ġbi ases Ġare Ġretrieved Ġif ĊĠĠĠĠ Ġ* Ġ[ Accel erometer Sensor Collector . Sensor Type . AC CEL ERO METER _ UN CAL IB R ATED ] Ġis Ġused . ĊĠĠĠĠ Ġ* ĊĠĠĠĠ Ġ* Ġ@ param Ġbx Ġx - coordinate Ġof Ġinitial Ġbias Ġto Ġbe Ġset Ġexpressed Ġin Ġmeters Ġper Ġsquared Ġsecond ĊĠĠĠĠ Ġ* Ġ( m / s ^ 2 ). ĊĠĠĠĠ Ġ* Ġ@ param Ġby Ġy - coordinate Ġof Ġinitial Ġbias Ġto Ġbe Ġset Ġexpressed Ġin Ġmeters Ġper Ġsquared Ġsecond ĊĠĠĠĠ Ġ* Ġ( m / s ^ 2 ). ĊĠĠĠĠ Ġ* Ġ@ param Ġb z Ġz - coordinate Ġof Ġinitial Ġbias Ġto Ġbe Ġset Ġexpressed Ġin Ġmeters Ġper Ġsquared Ġsecond ĊĠĠĠĠ Ġ* Ġ( m / s ^ 2 ). ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġprivate Ġfun Ġupdate Initial Bi ases ( bx : ĠFloat ?, Ġby : ĠFloat ?, Ġb z : ĠFloat ?) Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġinitial Bias X : ĠDouble ĊĠĠĠĠĠĠĠ Ġval Ġinitial Bias Y : ĠDouble ĊĠĠĠĠĠĠĠ Ġval Ġinitial Bias Z : ĠDouble ĊĠĠĠĠĠĠĠ Ġif Ġ( bx Ġ!= Ġnull Ġ&& Ġby Ġ!= Ġnull Ġ&& Ġb z Ġ!= Ġnull ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġinitial Bias X Ġ= Ġbx . toDouble () ĊĠĠĠĠĠĠĠĠĠĠĠ Ġinitial Bias Y Ġ= Ġby . toDouble () ĊĠĠĠĠĠĠĠĠĠĠĠ Ġinitial Bias Z Ġ= Ġb z . toDouble () ĊĠĠĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġinitial Bias X Ġ= Ġ 0 . 0 ĊĠĠĠĠĠĠĠĠĠĠĠ Ġinitial Bias Y Ġ= Ġ 0 . 0 ĊĠĠĠĠĠĠĠĠĠĠĠ Ġinitial Bias Z Ġ= Ġ 0 . 0 ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠ Ġthis . initial Bias X Ġ= Ġinitial Bias X ĊĠĠĠĠĠĠĠ Ġthis . initial Bias Y Ġ= Ġinitial Bias Y ĊĠĠĠĠĠĠĠ Ġthis . initial Bias Z Ġ= Ġinitial Bias Z ĊĊĠĠĠĠĠĠĠ Ġinitial Bias Available Listener ?. on Initial Bias Available ( ĊĠĠĠĠĠĠĠĠĠĠĠ Ġthis , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġinitial Bias X , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġinitial Bias Y , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġinitial Bias Z ĊĠĠĠĠĠĠĠ Ġ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠBuilds Ġan Ġinternal Ġaccel erometer Ġcal ibr ator Ġbased Ġon Ġall Ġprovided Ġparameters . ĊĠĠĠĠ Ġ* ĊĠĠĠĠ Ġ* Ġ@ return Ġan Ġinternal Ġaccel erometer Ġcal ibr ator . ĊĠĠĠĠ Ġ* Ġ@ throws ĠIllegalStateException Ġif Ġno Ġsuitable Ġcal ibr ator Ġcan Ġbe Ġbuilt . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġ@ Throws ( IllegalStateException :: class ) ĊĠĠĠ Ġprivate Ġfun Ġbuild Internal Cal ibr ator (): ĠAcc eler ometer Non Linear Cal ibr ator Ġ{ ĊĠĠĠĠĠĠĠ Ġreturn ĠAcc eler ometer Internal Cal ibr ator Builder ( ĊĠĠĠĠĠĠĠĠĠĠĠ Ġmeasurements , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġrobust Pre limin ary Sub setSize , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġminimum Required Measure ments , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġrobust Method , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġrobust Confidence , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġrobust Max Iterations , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġrobust Threshold , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġrobust Threshold Factor , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġrobust Stop Threshold Factor , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġlocation , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġgravity Norm , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġis Ground Truth Initial Bias , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġis Common Axis Used , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġinitial Bias X , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġinitial Bias Y , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġinitial Bias Z , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġinitial Sx , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġinitial Sy , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġinitial Sz , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġinitial M xy , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġinitial M xz , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġinitial My x , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġinitial My z , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġinitial Mz x , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġinitial M zy , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġbase Noise Level , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġquality Score Mapper ĊĠĠĠĠĠĠĠ Ġ). build () ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġcompanion Ġobject Ġ{ ĊĠĠĠĠĠĠĠ Ġ/** ĊĠĠĠĠĠĠĠĠ Ġ* ĠNumber Ġof Ġunknown s Ġwhen Ġcommon Ġz - axis Ġis Ġassumed Ġfor Ġboth Ġthe Ġaccel erometer Ġand ĊĠĠĠĠĠĠĠĠ Ġ* Ġgyro scope Ġand Ġbias Ġis Ġunknown . ĊĠĠĠĠĠĠĠĠ Ġ*/ ĊĠĠĠĠĠĠĠ Ġprivate Ġconst Ġval ĠUNKNOWN _ BI AS _ COMMON _ Z _ AXIS _ UN KNO WNS Ġ= Ġ 9 ĊĊĠĠĠĠĠĠĠ Ġ/** ĊĠĠĠĠĠĠĠĠ Ġ* ĠNumber Ġof Ġunknown s Ġfor Ġthe Ġgeneral Ġcalibration Ġcase Ġwhen Ġbias Ġis Ġunknown . ĊĠĠĠĠĠĠĠĠ Ġ*/ ĊĠĠĠĠĠĠĠ Ġprivate Ġconst Ġval ĠUNKNOWN _ BI AS _ GENERAL _ UN KNO WNS Ġ= Ġ 1 2 ĊĊĠĠĠĠĠĠĠ Ġ/** ĊĠĠĠĠĠĠĠĠ Ġ* ĠNumber Ġof Ġunknown s Ġwhen Ġcommon Ġz - axis Ġis Ġassumed Ġfor Ġboth Ġthe Ġaccel erometer Ġand ĊĠĠĠĠĠĠĠĠ Ġ* Ġgyro scope Ġand Ġbias Ġis Ġknown . ĊĠĠĠĠĠĠĠĠ Ġ*/ ĊĠĠĠĠĠĠĠ Ġprivate Ġconst Ġval ĠK NO WN _ BI AS _ COMMON _ Z _ AXIS _ UN KNO WNS Ġ= Ġ 6 ĊĊĠĠĠĠĠĠĠ Ġ/** ĊĠĠĠĠĠĠĠĠ Ġ* ĠNumber Ġof Ġunknown s Ġfor Ġthe Ġgeneral Ġcalibration Ġcase Ġwhen Ġbias Ġis Ġknown . ĊĠĠĠĠĠĠĠĠ Ġ*/ ĊĠĠĠĠĠĠĠ Ġprivate Ġconst Ġval ĠK NO WN _ BI AS _ GENERAL _ UN KNO WNS Ġ= Ġ 9 ĊĊĠĠĠĠĠĠĠ Ġ/** ĊĠĠĠĠĠĠĠĠ Ġ* ĠRequired Ġminimum Ġnumber Ġof Ġmeasurements Ġwhen Ġcommon Ġz - axis Ġis Ġassumed Ġand Ġbias Ġis ĊĠĠĠĠĠĠĠĠ Ġ* Ġunknown . ĊĠĠĠĠĠĠĠĠ Ġ*/ ĊĠĠĠĠĠĠĠ Ġconst Ġval ĠUNKNOWN _ BI AS _ MIN IMUM _ MEAS UREMENT S _ COMMON _ Z _ AXIS Ġ= ĊĠĠĠĠĠĠĠĠĠĠĠ ĠUNKNOWN _ BI AS _ COMMON _ Z _ AXIS _ UN KNO WNS Ġ+ Ġ 1 ĊĊĠĠĠĠĠĠĠ Ġ/** ĊĠĠĠĠĠĠĠĠ Ġ* ĠRequired Ġminimum Ġnumber Ġof Ġmeasurements Ġfor Ġthe Ġgeneral Ġcase Ġwhen Ġbias Ġis Ġunknown . ĊĠĠĠĠĠĠĠĠ Ġ*/ ĊĠĠĠĠĠĠĠ Ġconst Ġval ĠUNKNOWN _ BI AS _ MIN IMUM _ MEAS UREMENT S _ GENERAL Ġ= ĠUNKNOWN _ BI AS _ GENERAL _ UN KNO WNS Ġ+ Ġ 1 ĊĊĠĠĠĠĠĠĠ Ġ/** ĊĠĠĠĠĠĠĠĠ Ġ* ĠRequired Ġminimum Ġnumber Ġof Ġmeasurements Ġwhen Ġcommon Ġz - axis Ġis Ġassumed Ġand Ġbias Ġis Ġknown . ĊĠĠĠĠĠĠĠĠ Ġ*/ ĊĠĠĠĠĠĠĠ Ġconst Ġval ĠK NO WN _ BI AS _ MIN IMUM _ MEAS UREMENT S _ COMMON _ Z _ AXIS Ġ= ĊĠĠĠĠĠĠĠĠĠĠĠ ĠK NO WN _ BI AS _ COMMON _ Z _ AXIS _ UN KNO WNS Ġ+ Ġ 1 ĊĊĠĠĠĠĠĠĠ Ġ/** ĊĠĠĠĠĠĠĠĠ Ġ* ĠRequired Ġminimum Ġnumber Ġof Ġmeasurements Ġfor Ġthe Ġgeneral Ġcase Ġwhen Ġbias Ġis Ġknown . ĊĠĠĠĠĠĠĠĠ Ġ*/ ĊĠĠĠĠĠĠĠ Ġconst Ġval ĠK NO WN _ BI AS _ MIN IMUM _ MEAS UREMENT S _ GENERAL Ġ= ĠK NO WN _ BI AS _ GENERAL _ UN KNO WNS Ġ+ Ġ 1 ĊĊĠĠĠĠĠĠĠ Ġ/** ĊĠĠĠĠĠĠĠĠ Ġ* ĠDefault Ġconfidence Ġto Ġfind Ġa Ġrobust Ġcalibration . ĠBy Ġdefault Ġthis Ġis Ġ 9 9 %. ĊĠĠĠĠĠĠĠĠ Ġ*/ ĊĠĠĠĠĠĠĠ Ġconst Ġval ĠRO BU ST _ DEFAULT _ CONF ID ENCE Ġ= Ġ 0 . 9 9 ĊĊĠĠĠĠĠĠĠ Ġ/** ĊĠĠĠĠĠĠĠĠ Ġ* ĠDefault Ġmaximum Ġallowed Ġnumber Ġof Ġiterations Ġto Ġfind Ġa Ġrobust Ġcalibration Ġsolution . ĊĠĠĠĠĠĠĠĠ Ġ*/ ĊĠĠĠĠĠĠĠ Ġconst Ġval ĠRO BU ST _ DEFAULT _ MAX _ ITER ATIONS Ġ= Ġ 5 0 0 0 ĊĊĠĠĠĠĠĠĠ Ġ/** ĊĠĠĠĠĠĠĠĠ Ġ* ĠDefault Ġfactor Ġto Ġbe Ġused Ġrespect Ġestimated Ġbase Ġnoise Ġlevel Ġto Ġconsider Ġa Ġmeasurement ĊĠĠĠĠĠĠĠĠ Ġ* Ġan Ġout lier Ġwhen Ġusing Ġrobust Ġcalibration Ġmethods . ĊĠĠĠĠĠĠĠĠ Ġ* ĠBy Ġdefault Ġthis Ġis Ġ 3 . 0 Ġtimes Ġ[ base Noise Level ], Ġwhich Ġconsidering Ġthe Ġnoise Ġlevel Ġas ĊĠĠĠĠĠĠĠĠ Ġ* Ġthe Ġstandard Ġdeviation Ġof Ġa ĠGaussian Ġdistribution , Ġshould Ġaccount Ġfor Ġ 9 9 % Ġof Ġthe Ġcases . ĊĠĠĠĠĠĠĠĠ Ġ* ĠAny Ġmeasurement Ġhaving Ġan Ġerror Ġgreater Ġthan Ġthat Ġin Ġthe Ġestimated Ġsolution , Ġwill Ġbe ĊĠĠĠĠĠĠĠĠ Ġ* Ġconsidered Ġan Ġout lier Ġand Ġbe Ġdiscarded . ĊĠĠĠĠĠĠĠĠ Ġ*/ ĊĠĠĠĠĠĠĠ Ġconst Ġval ĠDEFAULT _ RO BU ST _ THRESHOLD _ FACTOR Ġ= Ġ 3 . 0 ĊĊĠĠĠĠĠĠĠ Ġ/** ĊĠĠĠĠĠĠĠĠ Ġ* ĠAdditional Ġfactor Ġfor Ġrobust Ġmethods Ġbased Ġon ĠL Med S Ġor ĠP ROM ed S , Ġwhere Ġfactor Ġis Ġnot ĊĠĠĠĠĠĠĠĠ Ġ* Ġdirectly Ġrelated Ġto ĠL M SE , Ġbut Ġto Ġa Ġsmaller Ġvalue . ĊĠĠĠĠĠĠĠĠ Ġ*/ ĊĠĠĠĠĠĠĠ Ġconst Ġval ĠDEFAULT _ RO BU ST _ STOP _ THRESHOLD _ FACTOR Ġ= Ġ 1 e - 2 ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠInterface Ġto Ġnotify Ġwhen Ġgravity Ġnorm Ġestimation Ġis Ġun reli able . ĊĠĠĠĠ Ġ* ĠThis Ġonly Ġhappens Ġif Ġno Ġlocation Ġis Ġprovided Ġand Ġgravity Ġsensor Ġbecomes Ġun reli able . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġinterface ĠOn Un reli able Gravity Estimation Listener Ġ{ ĊĠĠĠĠĠĠĠ Ġ/** ĊĠĠĠĠĠĠĠĠ Ġ* ĠCalled Ġwhen Ġgravity Ġnorm Ġestimation Ġbecomes Ġun reli able . ĊĠĠĠĠĠĠĠĠ Ġ* ĠThis Ġonly Ġhappens Ġif Ġno Ġlocation Ġis Ġprovided Ġand Ġgravity Ġsensor Ġbecomes Ġun reli able . ĊĠĠĠĠĠĠĠĠ Ġ* ĊĠĠĠĠĠĠĠĠ Ġ* Ġ@ param Ġcal ibr ator Ġcal ibr ator Ġthat Ġraised Ġthe Ġevent . ĊĠĠĠĠĠĠĠĠ Ġ*/ ĊĠĠĠĠĠĠĠ Ġfun Ġon Un reli able Gravity Estimation ( cal ibr ator : ĠSingle Sensor Static Interval Accel erometer Cal ibr ator ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠInterface Ġto Ġnotify Ġwhen Ġinitial Ġbias Ġguess Ġis Ġavailable . ĊĠĠĠĠ Ġ* ĠIf Ġ[ is Ground Truth Initial Bias ] Ġis Ġtrue , Ġthen Ġinitial Ġbias Ġis Ġconsidered Ġthe Ġtrue Ġvalue Ġafter ĊĠĠĠĠ Ġ* Ġsolving Ġcalibration , Ġotherwise , Ġinitial Ġbias Ġis Ġconsidered Ġonly Ġan Ġinitial Ġguess . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġinterface ĠOn Initial Bias Available Listener Ġ{ ĊĠĠĠĠĠĠĠ Ġ/** ĊĠĠĠĠĠĠĠĠ Ġ* ĠCalled Ġwhen Ġinitial Ġbias Ġis Ġavailable . ĊĠĠĠĠĠĠĠĠ Ġ* ĠIf Ġ[ is Ground Truth Initial Bias ] Ġis Ġtrue , Ġthen Ġinitial Ġbias Ġis Ġconsidered Ġthe Ġtrue Ġvalue ĊĠĠĠĠĠĠĠĠ Ġ* Ġafter Ġsolving Ġcalibration , Ġotherwise , Ġinitial Ġbias Ġis Ġconsidered Ġonly Ġan Ġinitial Ġguess . ĊĠĠĠĠĠĠĠĠ Ġ* ĊĠĠĠĠĠĠĠĠ Ġ* Ġ@ param Ġcal ibr ator Ġcal ibr ator Ġthat Ġraised Ġthe Ġevent . ĊĠĠĠĠĠĠĠĠ Ġ* Ġ@ param Ġbias X Ġx - coordinate Ġof Ġbias Ġexpressed Ġin Ġmeters Ġper Ġsquared Ġsecond Ġ( m / s ^ 2 ). ĊĠĠĠĠĠĠĠĠ Ġ* Ġ@ param Ġbias Y Ġy - coordinate Ġof Ġbias Ġexpressed Ġin Ġmeters Ġper Ġsquared Ġsecond Ġ( m / s ^ 2 ). ĊĠĠĠĠĠĠĠĠ Ġ* Ġ@ param Ġbias Z Ġz - coordinate Ġof Ġbias Ġexpressed Ġin Ġmeters Ġper Ġsquared Ġsecond Ġ( m / s ^ 2 ). ĊĠĠĠĠĠĠĠĠ Ġ*/ ĊĠĠĠĠĠĠĠ Ġfun Ġon Initial Bias Available ( ĊĠĠĠĠĠĠĠĠĠĠĠ Ġcal ibr ator : ĠSingle Sensor Static Interval Accel erometer Cal ibr ator , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġbias X : ĠDouble , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġbias Y : ĠDouble , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġbias Z : ĠDouble ĊĠĠĠĠĠĠĠ Ġ) ĊĠĠĠ Ġ} Ċ }