Data Fusion of Dual Foot-Mounted INS to Reduce the Systematic Heading Drift G.V. Prateek ⋆ , Girisha R ⋆ , K.V.S. Hari ⋆ and Peter H¨ andel † prateekgv@ece.iisc.ernet.in, girishr@ece.iisc.ernet.in, hari@ece.iisc.ernet.in and ph@kth.se http://ece.iisc.ernet.in/ ∼ ssplab ⋆ Statistical Signal Processing Laboratory, Department of ECE Indian Institute of Science, Bangalore, India. † Signal Processing Lab, ACCESS Linnaeus Centre, KTH Royal Institute of Technology, Stockholm, Sweden. ISMS 2013, Bangkok, Thailand 29 th January 2013 Funded by Department of Science and Technology, Govt. of India and VINNOVA, Sweden. ISMS-2013 Sphere Limit Method - Prateek, Nijil, Hari and Peter 1 of 22
Background Figure: First Responder System ISMS-2013 Sphere Limit Method - Prateek, Nijil, Hari and Peter 2 of 22
First Responder System The different systems tracks the states of different points on the body. There is a non-rigid relationship between the navigation points. There is an upper limit γ how spatially separated the systems can be. Figure: Illustration of the possible placements of the subsystem in a pedestrian navigation system and the maximum spatial separation γ between the subsystems. Image source: I. Skog, J.-O. Nilsson and P. Handel, “Fusing the information from two navigation systems using an upper bound on their maximum spatial separation,” to appear in IPIN dec 2012. ISMS-2013 Sphere Limit Method - Prateek, Nijil, Hari and Peter 3 of 22
Gait Cycle Phases Figure: Gait cycle phases during walking and running - (blue) push-off, (green) swing, (red) heel-strike, (orange) stance Image Source: Kwakkel, S.P., Lachapelle, G., Cannon, M.E., “GNSS Aided In Situ Human Lower Limb Kinematics During Running,” Proceedings of the 21st International Technical Meeting of the Satellite Division of The Institute of Navigation (ION GNSS 2008) , Savannah, GA, September 2008, pp. 1388-1397. ISMS-2013 Sphere Limit Method - Prateek, Nijil, Hari and Peter 4 of 22
Problem Description The main drawback of the existing 1 foot-mounted ZUPT aided INS is the Systematic Heading Drift. The estimated trajectories drift away from 2 the actual path as time progresses (despite having a calibration phase). (a) Building an OpenShoe Unit 3 One possible way these errors can be mitigated is to use foot-mounted INS on both feet such that the symmetrical modeling errors cancel out. In our paper, we have assumed the 4 maximum separation between the two foot-mounted IMUs as γ . (b) Shoe equipped with OpenShoe unit For more details visit http://openshoe.org ISMS-2013 Sphere Limit Method - Prateek, Nijil, Hari and Peter 5 of 22
Systematic Heading Drift - Simulation Figure: OpenShoe unit mounted on the left foot of a user asked to walk on a level path in the first floor of the Signal Processing Building, Indian Institute of Science, Bangalore, India. ISMS-2013 Sphere Limit Method - Prateek, Nijil, Hari and Peter 6 of 22
Observations The duration of the phases 1 of the gait cycle for walkers shows that for more than 50% of the time the foot occupies Heel-strike and Stance phase. And the errors are minimized when the foot is stationary. (ZUPT occurrences) Figure: Illustration of the motion of the feet during motion. The sphere indicates the range constraint on the spatial separation between the two feet with the feet that is stationary as the center of the sphere. 1Kwakkel, S.P., Lachapelle, G., Cannon, M.E., “GNSS Aided In Situ Human Lower Limb Kinematics During Running,” Proceedings of the 21st International Technical Meeting of the Satellite Division of The Institute of Navigation (ION GNSS 2008) , Savannah, GA, September 2008, pp. 1388-1397. ISMS-2013 Sphere Limit Method - Prateek, Nijil, Hari and Peter 7 of 22
Zero-Velocity Updates Left foot Zupt applied 1 Left ZUPT on/off 0.5 0 2.5 3 3.5 4 4.5 5 time [s] Right foot Zupt applied 1 Right ZUPT on/off 0.5 0 2.5 3 3.5 4 4.5 5 time [s] Figure: A snapshot of the ZUPTs occurrences for left and right foot from time instance 2.5[s] to 5[s] for a Straight Path trajectory using the GLRT algorithm 2 2I. Skog, P. Handel, J. Nilsson, and J. Rantakokko, “Zero-velocity detection - an algorithm evaluation,” Biomedical Engineering, IEEE Transactions on , vol. 57, pp. 2657 –2666, nov. 2010. ISMS-2013 Sphere Limit Method - Prateek, Nijil, Hari and Peter 8 of 22
Sphere Limit Method - Position Correction Illustration (a) Right foot position (b) Left foot position estimates after correction. estimates after correction. Figure: Cross section of a sphere of radius γ , which is the maximum possible spatial separation between the two feet. ISMS-2013 Sphere Limit Method - Prateek, Nijil, Hari and Peter 9 of 22
Proposed Solution - Position Correction x j Let d i x i k = norm ([ ˆ k ] 1:3 − [ ˆ k ] 1:3 ) represent the separation between the two navigation systems x i i , j ∈ { l , r } and i � = j , at any given instance of time k where ˆ k is a 9-state vector consisting of position, velocity and attitude information. If the i th navigation systems is in stance phase (ZuPT is ON), the j th navigation system is not in stance phase and the separation between them is d j k > γ , then the new position coordinates of the j th navigation system is obtained as follows 1 p j � ( d i x i x j � ˆ = k − γ )[ ˆ k ] 1:3 + γ [ ˆ k ] 1:3 (1) . k d i k Equation (1) represents the orthogonal projections of the position estimates of the foot that is in motion on to the surface of the sphere. ISMS-2013 Sphere Limit Method - Prateek, Nijil, Hari and Peter 10 of 22
Existing Algorithm - Pseudo Code Pseudo code for the algorithm without range constraint on the spatial separation of the two navigation systems i , j where i , j ∈ { l , r } and i � = j . 1: k ← 0 k ← sample index. 2: P i k , Q i k , R i k , H i k ← Process { Initialize Filter } P k ← 9-state covariance matrix. 3: P j k , Q j k , R j k , H j k ← Process { Initialize Filter } k ) T } is the process noise Q k = E { w 1 k ( w 1 x i 4: ˆ k ← Process { Initial Nav State } due to gyroscope and accelerometer and x j 5: ˆ k ← Process { Initial Nav State } w 1 k ∈ R 6 . 6: loop k ) T } is the zupt occurrence 7: k ← k + 1 R k = E { w 2 k ( w 2 x i 8: ˆ k ← Process { Nav Equations } measurement noise and w 2 k ∈ R 3 . x j 9: ˆ k ← Process { Nav Equations } H k = � 0 3 I 3 0 3 � is the observation T + G i T P i k ← F i k P i k − 1 F i k Q i k G i 10: matrix for zupt algorithm. k k T + G j T P j k ← F j k P j k − 1 F j k Q j k G j 11: ˆ x k is the estimated 9-state vector k k 12: for s ∈ { l , r } do containing position, velocity and attitude if zupt s 13: k is on then estimates. T [ H s T + R s K s k ← P s k H s k P s k H s k ] − 1 14: k k F k and G k define the state space model. δ x s k ← − K s x s 15: k [ ˆ k ] 4:6 x s 16: ˆ k ← Process { Correct Nav States } l and r represent the left and right P s k ← [ I − K s k H s k ] P s 17: navigation system respectively. k 18: end if 19: end for 20: end loop ISMS-2013 Sphere Limit Method - Prateek, Nijil, Hari and Peter 11 of 22
Experimental Setup - 1 Signal Processing Building. First Floor Corridor Path. Data Collection 35 Collected the data on the C B first floor of Signal 30 2.07 23[m] 2.15 Processing Building, Indian Institute of Science, 25 Bangalore, India. 20 The tiles in the building y[m] 34[m] 34[m] were used as marker beacon 15 to collect data in a 10 controlled manner. Each tile of length 2 feet × 2 feet. 5 D A The maximum stride length 2.01 2.21 0 never exceeded 0.6096[m]. −5 Assumptions −25 −20 −15 −10 −5 0 x[m] The two IMUs are aligned in Figure: A layout of the first floor of the the same direction. Signal Processing Building. It is in inverted Sampling happens at full U shape. The parallel arms are 34[m] in speed (820 Hz). length and the perpendicular arm is 23[m] in length. ISMS-2013 Sphere Limit Method - Prateek, Nijil, Hari and Peter 12 of 22
Simulation Results - Existing Algorithm Inverted L Path Trajectory Inverted U Path Trajectory 40 40 35 C B C 35 B 30 30 25 25 position y [m] position y [m] 20 20 15 15 Right Leg Trajectory 10 Left Leg Trajectory 10 Right Leg Trajectory Start point Left Leg Trajectory Actual Path 5 5 Start point A D 0 A 0 −35 −30 −25 −20 −15 −10 −5 0 5 −35 −30 −25 −20 −15 −10 −5 0 5 10 position x [m] position x [m] (a) Left and Right foot trajectory for (b) Left and Right foot trajectory for Inverted ‘L’ Path along segment AB and BC. Inverted ‘U’ Path along segment AB, BC and CD Figure: Trajectories obtained after applying the algorithm without any range constraint on the spatial separation between two feet. Initial heading value is equal to 0 ◦ for all data sets. ISMS-2013 Sphere Limit Method - Prateek, Nijil, Hari and Peter 13 of 22
Recommend
More recommend
