InitializationofaPedestrianNavigationSystemUsingaVehicleNavigationSystem

ArchitThopayDavidBevlyScottMartinHowardChen

2019StanfordPNTSymposium

AGVLAB

PresentationOutline

•  Introduction–  Motivation–  ProblemDefinition–  ProposedSolution

•  TheoreticalBasis–  RigidityDetection–  OffsetCalculation

•  PerformanceAnalysis–  Subsystems–  CompleteSystem

•  ConclusionsandFutureWork

2

AGVLAB

FirstVisittoStanfordUniversity

3

HooverInstitution AndersonCenter

AGVLAB

UsingaMap

4

Iamhere

AGVLAB

GNSSPositioning

5

AGVLAB

GNSSWeaknesses

•  GoodGNSSsignalsnotalwaysavailable.–  Blockage–  Attenuation

•  Pedestriannavigationtechniquescanfillthegap.–  IndependentofGNSS

6

AGVLAB

PedestrianNavigationTechniques

PedestrianDeadReckoning•  Attitude

–  AngularVelocity–  Acceleration–  MagneticField

•  StepLength–  Inertial

InertialNavigation•  6-DOFangularandpositional

displacement–  Angularvelocity–  Acceleration

7

[1]

AGVLAB

InitializationProblem

PedestrianDeadReckoning

𝑥↓𝑖+1 = 𝑥↓𝑖 +(𝑆𝐿𝑐𝑜𝑠(𝜓))𝑦↓𝑖+1 = 𝑦↓𝑖 +(𝑆𝐿𝑠𝑖𝑛(𝜓))

InertialNavigation

𝐶 ↓𝑏↑𝑛 = 𝐶↓𝑏↑𝑛 [𝜔×]𝑣=∫↑▒𝐶↓𝑏↑𝑛 𝑓 ↓𝑏 𝑑𝑡 + 𝑣↓𝑖 𝑝=∫↑▒𝑣𝑑𝑡 + 𝑝↓𝑖 

8

InitialAttitude

InitialPositionandVelocity

AGVLAB

CurrentInitializationTechnique

Position PitchandRoll Heading

Standstilloutdoorsforoneminute

GNSS Accelerometer Magnetometer

[2]

9

AGVLAB10

UsetheVehicle’sSensors

•  Firstrespondersrideinvehicles.

–  Vehicles→Navigationsensors

•  AssumevehiclehasagoodPNTsolution.

•  Thepedestriannavigationsystemcanbeinitializedusingthevehicle’snavigationsystemasareference.

–  Pedestrian→InsideVehicle

AGVLAB11

ProposedSystem

1)  DetectwhenpedestrianIMUisrigidwithvehicleIMUduringthedrive.

2)CalculaterotationandtranslationbetweenvehicleandpedestrianIMUs.

3)Transformvehiclepositionandattitudeintopedestrian’sframe.

[𝑅|𝑡]

AGVLAB

TheoreticalBasis

RigidityDetectionandOffsetCalculation

12

AGVLAB13

RigidityDetectionBackground

•  DetectrigidperiodsusingIMUmeasurements–  Pedestrianmounted–  VehicleMounted

•  Similartofoot-mountedstepdetectionapproachesusedforpedestriannavigation.[2]

|𝑎|=√�𝑎↓𝑥↑2 + 𝑎↓𝑦↑2 + 𝑎↓𝑧↑2  

[3]

AGVLAB14

RigidityDetectionTheory

•  Differenceinangularvelocitymagnitudes.

•  Twopointsonrigidbodyexperiencesameangularvelocitymagnitude.

•  Ifdifferencefallsbelowthreshold,thenIMUscanbeassumedtorigidlyattached.

𝜔↓𝑣𝑒ℎ𝑖𝑐𝑙𝑒↑𝑛𝑜𝑟𝑚 (𝑘)=√�𝜔↓𝑥 (𝑘)↑2 + 𝜔↓𝑦 (𝑘)↑2 + 𝜔↓𝑧 (𝑘)↑2  

Ω↓𝑝𝑒𝑑𝑒𝑠𝑡𝑟𝑖𝑎𝑛↑𝑛𝑜𝑟𝑚 (𝑘)=√�Ω↓𝑥 (𝑘)↑2 +

Ω↓𝑦 (𝑘)↑2 + Ω↓𝑧 (𝑘)↑2  

𝑔↓𝑑𝑖𝑓𝑓↑𝑛𝑜𝑟𝑚 (𝑘)= 𝜔↓𝑣𝑒ℎ𝑖𝑐𝑙𝑒↑𝑛𝑜𝑟𝑚 (𝑘)− Ω↓𝑝𝑒𝑑𝑒𝑠𝑡𝑟𝑖𝑎𝑛↑𝑛𝑜𝑟𝑚 (𝑘)

𝑑𝑒𝑡𝑒𝑐𝑡↓𝑟𝑖𝑔𝑖𝑑↓𝑘  ={█𝑓𝑎𝑙𝑠𝑒&𝑖f

𝑔↓𝑑𝑖𝑓𝑓↑𝑛𝑜𝑟𝑚 |↓𝑘−𝑠↑𝑘 >𝑡ℎ↓𝑚𝑎𝑥 @𝑡𝑟𝑢𝑒&otherwise  

AGVLAB

OffsetCalculationBackground

15

PedestrianIMUVehicleIMU

AngularVelocity

SpecificForce

Vehicle 𝜔 ↓𝑣  𝑓 ↓𝑣 

Pedestrian 𝜔 ↓𝑝  𝑓 ↓𝑝 

AGVLAB16

AngularOffsetCalculation

•  Needtocalculaterotationmatrix𝑹betweenvehicleandpedestrianIMUs.

•  Wahba’sproblem–  Spacecraftattitudedetermination

[4]

𝜔 ↓𝑣 = 𝑹𝜔 ↓𝑝 𝐽= 1/2 𝑤↓𝑘 ‖𝜔 ↓𝑣 −𝑹𝜔 ↓𝑝 ‖↑2 

ReferenceVectors OffsetVectors

ExistingReferenceStarLocations

orMagneticFieldMeasuredStarLocations

orMagneticField

Proposed AngularVelocityfromVehicleIMU

AngularVelocityfromPedestrianIMU

AGVLAB17

Wahba’sProblem

•  AttitudeProfileMatrix[4]•  PossibleSolution

–  SingularValueDecomposition(SVD)

•  LessComputationallyIntenseSolutions–  QuaternionEstimator

(QUEST)–  FastOptimalAttitudeMatrix

(FOAM)–  EstimatoroftheOptimal

Quaterion(ESOQ)

𝐵=∑𝑘↑▒𝑤↓𝑘 𝜔 ↓𝑣 𝜔 ↓𝑝↑𝑇  

𝐵=𝑈 𝑑𝑖𝑎𝑔[𝑠↓1  𝑠↓2  𝑠↓3 ] 𝑉↑𝑇 𝑹=𝑈 𝑑𝑖𝑎𝑔[11det�(𝑈) det�(𝑉) ] 𝑉↑𝑇 

SVD

AGVLAB18

TranslationalOffsetCalculation

•  Needtocalculatetranslationvector𝒕betweenvehicleandpedestrianIMUs.

–  Assumesknowledgeof𝑹

𝑓 ↓𝑣 = 𝑹𝑓 ↓𝑝 +( 𝜔 ↓𝑣  ×(𝜔 ↓𝑣  × 𝑡 ))+( 𝜔 ↓𝑣   × 𝑡 )

[█𝑓↓𝑥 @𝑓↓𝑦 @𝑓↓𝑧  ]↓𝑣 −𝑹[█𝑓↓𝑥 @𝑓↓𝑦 @𝑓↓𝑧  ]↓𝑝 =[█𝜔↓𝑦↑2 𝜔↓𝑧↑2 &𝜔 ↓𝑧 −𝜔↓𝑥 𝜔↓𝑦 &− 𝜔 ↓𝑦 −𝜔↓𝑥 𝜔↓𝑧 @− 𝜔 ↓𝑧 −𝜔↓𝑥 𝜔↓𝑦 &𝜔↓𝑥↑2 𝜔↓𝑧↑2 &𝜔 ↓𝑥 −𝜔↓𝑦 𝜔↓𝑦 @𝜔 ↓𝑦 −𝜔↓𝑥 𝜔↓𝑧 &− 𝜔 ↓𝑥 −𝜔↓𝑦 𝜔↓𝑧 &𝜔↓𝑥↑2 𝜔↓𝑦↑2  ][█𝒕↓𝒙 @𝒕↓𝒚 @𝒕↓𝒛  ]

𝑦 𝐻 𝑥

𝑦=𝐻𝑥𝑥 = ( 𝐻↑𝑇 𝐻)↑−1 𝐻↑𝑇 𝑦 [5]

AGVLAB

DeterministicObservability

•  Calculationofoffsetsdependoninertialmeasurementoutputs.

–  Inertialmeasurements→VehicularMotion

•  RotationalOffset

–  Wahba’sProblem→Twolinearlyindependentreferencevectors[4]

–  Linearlyindependentrotationintwoaxes

•  TranslationalOffset–  Measurementmodelmustbefullrank–  Nonzeroangularvelocityand

accelerationinatleasttwoaxes.

•  Lanechanging,turning

[█[█𝜔↓𝑦↑2 𝜔↓𝑧↑2 &𝜔 ↓𝑧 −𝜔↓𝑥 𝜔↓𝑦 &− 𝜔 ↓𝑦 −𝜔↓𝑥 𝜔↓𝑧 @− 𝜔 ↓𝑧 −𝜔↓𝑥 𝜔↓𝑦 &𝜔↓𝑥↑2 𝜔↓𝑧↑2 &𝜔 ↓𝑥 −𝜔↓𝑦 𝜔↓𝑦 @𝜔 ↓𝑦 −𝜔↓𝑥 𝜔↓𝑧 &− 𝜔 ↓𝑥 −𝜔↓𝑦 𝜔↓𝑧 &𝜔↓𝑥↑2 𝜔↓𝑦↑2  ]↓1 @⋮@[█𝜔↓𝑦↑2 𝜔↓𝑧↑2 &𝜔 ↓𝑧 −𝜔↓𝑥 𝜔↓𝑦 &− 𝜔 ↓𝑦 −𝜔↓𝑥 𝜔↓𝑧 @− 𝜔 ↓𝑧 −𝜔↓𝑥 𝜔↓𝑦 &𝜔↓𝑥↑2 𝜔↓𝑧↑2 &𝜔 ↓𝑥 −𝜔↓𝑦 𝜔↓𝑦 @𝜔 ↓𝑦 −𝜔↓𝑥 𝜔↓𝑧 &− 𝜔 ↓𝑥 −𝜔↓𝑦 𝜔↓𝑧 &𝜔↓𝑥↑2 𝜔↓𝑦↑2  ]↓𝑘  ]

19

𝜔 ↓𝑣↓1  =[█1@0@0 ]𝑟𝑎𝑑/𝑠, 𝜔 ↓𝑣↓2  =[█0@1@0 ]𝑟𝑎𝑑/𝑠,𝜔 ↓𝑣↓2  =[█0@1@0 ]𝑟𝑎𝑑/𝑠

AGVLAB

DoubleLaneChangeSimulation

•  AngularandtranslationaloffsetcalculationtestedusingvehicleIMUdatasimulatedinCarsim.

•  TrueAngularOffset: [50 30 −20]↑° •  TrueLeverArm:[1.524 0.938 0.8128] 𝑚

20

AGVLAB

SimulationResults

•  ReferenceandOffsetIMUdatawasusedtocalculaterotationalandtranslationaloffsets.

•  Maneuvercausesestimatestoconvergetotruevalues

21

AGVLAB

PerformanceAnalysis

22

SubsystemsandTotalSystem

AGVLAB

DataCollectionSetup

23

TacticalGradeIMU

MEMSGradeIMUonfoot

MEMSGradeIMUonvehicle

AGVLAB

RigidityDetectionTestandResults

•  Footwasmovedatvaryingintervals.

•  Buttonpushedatbeginningandendofmovementintervals.

24

RigidPeriod

Movement

ButtonPushes

Still-MovementInterval(sec)

PercentRigid

Detected

PercentMovementDetected

30 97% 99%

20 98% 99%

10 95% 97%

AGVLAB

OffsetCalculationResults

AGVLAB

InitializingPedNavSystem

1)  Detectrigidity2)  Estimateoffsetsduring

rigidperiods.3)  Transformvehiclenav

statesintopedestrianframe

4)  Initializepedestriannavigationalgorithm.

5)  Pedestrianexitedandwalkedstraightaway.

26

Starting/DropOffPoint

AGVLAB

RunI–MinimalMovement

27

AGVLAB

RunI–MinimalMovement

•  InitialpednavcoursecloselymatchesGPScourse.

•  Initializedheadingbetterthanassumingpedestrianisperpendiculartovehicleheading

28

𝑥

𝑦𝑥

𝑦

InitializedError PerpendicularError

1° 22°

𝟐𝟐°

AGVLAB

RunII–MoreMovement

29

Imovedmyfoot

AGVLAB

RunII–MoreMovement

•  InitialpednavcoursedoesnotmatchGPScourse.–  Moreaccurateinitializationin

RunI.

•  Movingfoot-mountedIMU15secondsbeforevehicleexitinvalidatesinitialization.–  Notenoughmotiontore-establish

angularoffset

30

𝟐𝟔°

AGVLAB

•  TwoIMUsrigidlymountedtothevehicle.•  Calculateangularoffsetinsideeachdatawindow;calculatedsolutionerror.

•  DifferentRigidPeriod/DataWindowSizes:60,50,40,30,20,10,and5seconds

HowOftenisThereExcitation?

31

Error:+ [𝟏𝟎° 𝟑° −𝟐°]

DrivingEnvironment:CityLength:1hourTrueOffset:[𝟗𝟎° 𝟒𝟓° −𝟒𝟓°]

Error:+ [−𝟑° 𝟕° 𝟓°]

AGVLAB

AttitudeEstimationErrorStatistics

•  Accuracyandprecisiondecreasewithsmallerrigidperiods/datawindowsizes.

•  Shorterrigidperiods→Lesslikelyfortheretobesufficientexcitation

32

AGVLAB

AttitudeEstimationErrorStatistics•  Accuracyandprecisiondecreasewithsmallerrigidperiods/datawindow

sizes.•  Shorterrigidperiods→Lesslikelyfortheretobesufficientexcitation

33

1°>10° 5°

>20°

AGVLAB

ConclusionandFurtherWork

•  AngularandlinearoffsetsbetweenpedestrianandvehicleIMUscanbeaccuratelycalculated.

•  Offsetcalculationcanyieldaccurateinitializationinformationforpedestriannavigation.–  Rigidperiods– Maneuverbeforevehicleexit

•  Furtherwork– MechanizeIMUoutputduringnon-rigidperiods–  RigidlymountpedestrianIMUtovehicle

34

AGVLAB

Questions?

ThankYou!

35

AGVLAB

References

[1]P.Groves,G.Pulford,C.AaronLittlefield,D.LJNash,andC.JMather,InertialNavigationVersusPedestrianDeadReckoning:OptimizingtheIntegration,vol.2.2007.[2]N.Strozzi,F.Parisi,andG.Ferrari,“Onsinglesensor-basedinertialnavigation,”2016IEEE13thInternationalConferenceonWearableandImplantableBodySensorNetworks(BSN),2016.[3]I.Skog,P.Handel,J.O.Nilsson,andJ.Rantakokko,“Zero-VelocityDetection—AnAlgorithmEvaluation,”IEEETransactionsonBiomedicalEngineering,vol.57,no.11,pp.2657–2666,2010.[4]Markley,Landis,“30YearsofWahba’sProblem,”presentedattheFlightMechanics,Greenbelt,MD;UnitedStates,1999.[5]M.Crabolu,D.Pani,L.Raffo,M.Conti,P.Crivelli,andA.Cereatti,“Invivoestimationoftheshoulderjointcenterofrotationusingmagneto-inertialsensors:MRI-basedaccuracyandrepeatabilityassessment,”BiomedEngOnline,vol.16,no.1,pp.34–34,Mar.2017.36