Indoor Pedestrian Navigation using an INS/EKF framework for Yaw ...

equation:δx k|k = δx k|k−1 + K k ·[m k − Hδx k|k−1 ], (16)where K k is the Kalman gain, m k is the actual error measurement,and δx k|k−1 is the predicted error state.The Kalman gain is calculated during the correction phasewith the usual formula:K k = P k|k−1 H T (HP k|k−1 H T + R k ) −1 . (17)where P k|k−1 is the estimation error covariance matrix, thatis computed at time k based on measurements received at timek-1, in this way: P k|k−1 = Φ k−1 P k−1|k−1 Φ T k−1 + Q k−1. Thecovariance matrix P k|k at time k is then computed using theKalman gain in the Joseph form equation: P k|k = (I 15×15 −K k H)P k|k−1 (I 15×15 − K k H) T + K k R k K T k . Similarly, duringthe update phase after corrections with measurements at timek, the predicted covariance matrix for time k+1 is computedas: P k+1|k = Φ k P k|k Φ T k + Q k.It is important to mention that the non-bias error terms ofthe filtered state vector, δx k|k , are reset to zero after the INSuses them to refine the current attitude, velocity and position.This is because those errors are already compensated andincorporated into the INS estimations. The only terms thatare maintained over time in the EKF filter are the gyro andaccelerometer biases, i.e. δω b k and δab k .The actual error measurement m k that feeds the EKF, iscalculated for ZUPT as: m k = v k|k−1 − [0,0,0], wherethe zero vector means that at stance we know that thevelocity of the foot is almost zero. The measurement matrix,H, for ZUPT update is a 3 by 15 matrix like this:H = [0 3×3 , 0 3×3 , 0 3×3 , I 3×3 , 0 3×3 ]. It selects the velocityerror components from the error state matrix δx k , i.e. the 10thto 12th terms.C. Stance detectionThe EKF gets feedback from measurements, only when theperson’s foot is detected to be stationary on the ground (totallystill, or in a stance phase during walk). Most algorithms in theliterature for stance detection rely on basic signal processingtechniques with accelerometers [5], [13] or gyroscopes [2].They properly work in many situations but occasionally failin slow and random walk [13]. We implement a multiconditionalgorithm, that complements the implementation of[10] by using both sources of information (accelerometers andgyroscopes) and an order filter so as to make the detectionprocess robust enough (see Fig. 3).The three conditions (C1, C2 and C3) to declare a foot asstationary are:1) The magnitude of the acceleration, |a k | = [a b k (1)2 +a b k (2)2 + a b k (3)2 ] 0.5 must be between two thresholds(th amin = 9 and th amax = 11 m/s 2 ).C1 ={1 thamin < |a k | < th amax0 otherwise. (18)Stance & Still phase detectionC1: Magnitude of acceleration conditionC2: Local variance of acceler. conditionC3: Angular rate conditionFig. 3.AccelerationIMUAngular rateAND Median filter Stance & StilldetectionEvent detectionSetup measurement for EKFStance and Still detection block used in the IEZ framework.2) The local acceleration variance, which highlights thefoot activity, must be above a given threshold (th σa =3 m/s 2 ). The local variance is computed this way:σ 2 a b k= 12s+1k+s∑j=k−s(a b k −a b k )2 , (19)where a b kis a local mean acceleration value, computedby this expression:a b k = 1 ∑ k+s2s+1 q=k−s a q, andsdefinesthe size of the averaging window (s = 15 samples). Thesecond condition C2 is satisfied when:C2 ={ 1 σa bk> th σa0 otherwise. (20)3) The magnitude of the gyroscope, |ω k | = [ω b k (1)2 +ω b k (2)2 + ω b k (3)2 ] 0.5 , must be below a given threshold(th ωmax = 50 o /s).C3 ={ 1 |ωk | < th ωmax0 otherwise. (21)The three logical conditions must be satisfied simultaneouslyfor foot stationary detection, so a logical “AND” isapplied, and the result is filtered out using a median filter witha neighboring window of 11 samples in total. The registeredlogical “1”s mark the Stance phase that occurs when the foot isstationary on the floor (while walking, or not). The Still phase,corresponding to a non-walking stationary foot, is detectedfrom stance samples that are not surrounded by non-stancesamples in a window larger than 2 seconds. Figure 4 showsresults of this multi-condition stance detection process.III. METHODS TO REDUCE THE DRIFT IN HEADINGThe IEZ Kalman-based PDR method presented in lastsection (Fig. 1), accumulates large errors in orientation dueto the bias in the gyroscopes. IEZ can not estimate it becausethe yaw orientation (ψ k ), and the gyroscopic bias (δω b ) in thevertical axis, are not observable from ZUPT measurementsalone [14]. Next subsections describe the integration in IEZof some approaches to reduce the drift in heading: ZARU[10], HDR [12], and the use of magnetometers as a compass.We focus on methods to reduce the drift without usingany external infrastructure such as GPS and LPS, nor mapmatchingtechniques. See Fig. 5 to view how these methodsare integrated in the IEZ framework.