Data Fusion of Dual Foot-Mounted Zero Velocity Update (ZUPT) Aided Inertial Navigation Systems (INSs) using Centroid Method

Transcription

1 February 02, 2013 Data Fusion of Dual Foot-Mounted Zero Velocity Update (ZUPT) Aided Inertial Navigation Systems (INSs) using Centroid Method Girisha Under the guidance of Prof. K.V.S. Hari

2 Notations Define the navigation state vector of right foot INS at time instant N + [ ] ˆx (R) = ˆp (R) ˆθ (R) T (R) ˆp ˆv (R) ˆθ(R) ˆv (R) R 3 is the position estimate of right foot INS R 3 is the velocity estimate of right foot INS R 3 is the attitude estimate of right foot INS

3 Notations Define the navigation state vector of right foot INS at time instant N + [ ] ˆx (R) = ˆp (R) ˆθ (R) T (R) ˆp ˆv (R) ˆθ(R) ˆv (R) R 3 is the position estimate of right foot INS R 3 is the velocity estimate of right foot INS R 3 is the attitude estimate of right foot INS Define the navigation state vector of left foot INS at time instant N + [ ] ˆx (L) T = ˆp (L) ˆθ (L) (L) ˆp ˆv (L) ˆθ(L) ˆv (L) R 3 is the position estimate of left foot INS R 3 is the velocity estimate of left foot INS R 3 is the attitude estimate of left foot INS

4 Range Constraint Algorithm ZUPT Aided INS (Right Foot) ZUPT Aided INS (Left Foot) [ [ ] ˆx (R) ] ˆx (L) Range Constraint Algorithm [ [ ] ˆx (R) ] ˆx (L) At any instant of time N +, the distance between position estimates of right and left foot INS cannot be greater than a parameter nown as foot-to-foot maximum separation (γ). Based on this range constraint, we develop Centroid Method to fuse the navigation information of right and left foot-mounted ZUPT aided INS.

5 Centroid Method - Problem Formulation Define the unconstrained joint position estimate vector at time instant N + ˆ [ = ˆp (R) ] T ˆp (L) (R) ˆp ˆp (L) R 3 is the position estimate of right foot INS R 3 is the position estimate of left foot INS

6 Centroid Method - Problem Formulation Define the unconstrained joint position estimate vector at time instant N + ˆ [ = ˆp (R) ] T ˆp (L) (R) ˆp ˆp (L) R 3 is the position estimate of right foot INS R 3 is the position estimate of left foot INS Define the matrix L = [ I 3 I 3 ] where I q is the identity matrix of size q.

7 Centroid Method - Problem Formulation At any instant of time N +, following condition must hold: Lˆ 2 ˆp = (R) ˆp (L) γ2 where γ R + is the foot-to-foot maximum separation

8 Centroid Method - Problem Formulation At any instant of time N +, following condition must hold: Lˆ 2 ˆp = (R) ˆp (L) γ2 where γ R + is the foot-to-foot maximum separation Hence, we can formulate the problem in constrained least squares framewor as: ( ) (joint) = argmin ˆp p 2 subject to L 2 p R γ2 (1) where [ = p (R) ] T p (L) is the range constrained joint position estimate vector at time instant N +

9 Centroid Method - Solution In Lagrangian formulation with Lagrange multiplier λ C : ( ) ( = (joint) J,λ C ˆp + λ C Ψ 2 2 ) where ( Ψ ) = L 2 2 γ2

10 Centroid Method - Solution In Lagrangian formulation with Lagrange multiplier λ C : ( ) ( = (joint) J,λ C ˆp + λ C Ψ 2 2 ) where ( Ψ ) = L 2 2 γ2 Lagrange condition (normal equations): ( ) J,λ C J (,λ C ) = 0 = ( I 6 + λ C L T L ) λ C = 0 = L = ˆ (2) 2 = 2 γ2 (3)

11 Centroid Method - Solution The solution of equation (2) is = ( I 6 + λ C L T L ) 1 ˆp (joint) (4)

12 Centroid Method - Solution The solution of equation (2) is = ( I 6 + λ C L T L ) 1 ˆp (joint) (4) Solve for λ C : First note that (I 6 + λ C L T L) 1 = 1 1+2λ C [ (1+λC ) I 3 λ C I 3 λ C I 3 (1+λ C ) I 3 ] (5) provided λ C 0.5

13 Centroid Method - Solution The solution of equation (2) is = ( I 6 + λ C L T L ) 1 ˆp (joint) (4) Solve for λ C : First note that (I 6 + λ C L T L) 1 = 1 1+2λ C [ (1+λC ) I 3 λ C I 3 λ C I 3 (1+λ C ) I 3 ] (5) provided λ C 0.5 Substitute Eq. (4) in Eq. (3) = L ( I 6 + λ C L T L ) 1 (joint) ˆp 2 = 2 γ2

14 Centroid Method - Solution Substitute Eq. (5) and simplifying: ( ) ˆd λ C = 0.5 γ 1 where ˆd = ˆp (R) ˆp (L) is the distance between the unconstrained position estimates of right and left foot INS. 2 (6)

15 Centroid Method - Solution Substitute Eq. (5) and simplifying: ( ) ˆd λ C = 0.5 γ 1 where ˆd = ˆp (R) ˆp (L) is the distance between the unconstrained position estimates of right and left foot INS. Substitute Eq. (5), Eq. (6) in Eq. (4) and simplifying: p (R) = p (L) = (ˆd + γ) (ˆd + γ) ˆp (R) + ˆp (L) + 2 (ˆd γ) ˆp (L) (6) 2 ˆd (7) (ˆd γ) ˆp (R) 2 ˆd (8)

16 Graphical Representation ˆp (R) p (R) Sphere of diameter γ and centre at p 0 p 0 Figure: Graphical representation of unconstrained and constrained position estimates p (L) ˆp (L)

17 Alternate Problem Formulation In the figure, the centre of the sphere Hence, by substituting p 0 = p(r) p (L) + p (L) 2 (9) = 2p 0 p (R) (10) an alternate formulation of (1) is written as ( ) p (R) (joint) = argmin ˆp L T p p R 3 2 subject to p (R) 2 p 0 γ2 2 4 where 0 = [ ] T 0 p 0

18 Alternate Problem Formulation Solution in Lagrangian framewor is found to be: p (R) = γ ˆp(R) γ ˆp (L) + 2 ˆd p 0 2 ˆd (11) Substitute Eq. (11) in Eq. (10) p (L) = γ ˆp(L) γ ˆp (R) + 2 ˆd p 0 2 ˆd (12) If we define the centroid ˆp (R) p 0 = + ˆp (L) 2 solution given by Eq. (11) and (12) reduces to the solution given by Eq. (7) and Eq. (8) respectively. (13)

19 Weighted Centroid Method Instead of defining the centroid as the midpoint of right and left foot INS position estimates as in Eq. (13), we can mae use of the entries of error covariance matrix P, to define the weighted centroid. Range constrained right and left foot position estimates given by Eq. (11) and (12) is used as the pseudo-measurement in complementary feedbac Kalman Filter.

20 Centroid Method - Algorithm Description If ˆd > γ, then execute the following steps. Calculate the pseudo-measurement for the position given by Eq. (11) and Eq. (12), where p 0 is given by Eq. (13) or as explained in weighted centroid method.

21 Centroid Method - Algorithm Description If ˆd > γ, then execute the following steps. Calculate the pseudo-measurement for the position given by Eq. (11) and Eq. (12), where p 0 is given by Eq. (13) or as explained in weighted centroid method. Kalman filter position pseudo-measurement update for right foot INS: 1. Compute the Kalman gain: K (R) = P (R) ( H (R) (pos) ) T [ H (R) (pos) P(R) ( ) ] T 1 H (R) (R) (pos) + R (pos) 2. Correct the navigation state vector using the position pseudo-measurement: [ ] ˆx (R) = ˆx (R) + K (R) p (R) H (R) (pos) ˆx(R) 3. Correct the state covariance matrix: P (R) = ( ) I K (R) H (R) (pos) P (R)

22 Centroid Method - Algorithm Description Kalman filter position pseudo-measurement update for left foot INS: 1. Compute the Kalman gain: K (L) = P (L) ( H (L) (pos) ) [ T H (L) (pos) P(L) ( ) ] T 1 H (L) (L) (pos) + R (pos) where H (L) (pos) = [ ] I is the position pseudo-measurement observation matrix of left foot INS and is the position pseudo-measurement noise covariance matrix of left foot INS. 2. Correct the navigation state vector using the position pseudo-measurement: R (L) (pos) ˆx (L) = ˆx (L) + K (L) 3. Correct the state covariance matrix: P (L) = [ p (L) ( ) I K (L) H (L) (pos) ] H (L) (pos) ˆx(L) P (L)

23 Than You