Pedestrian Dead-Reckoning Algorithms For Dual Foot-Mounted Inertial Sensors

This is a preprin t cop y that has b een accepted for publication in Pro ceedings of the 26th Sain t P etersburg In ternational Conference on In tegrated Na vigation Systems (ICINS). © 2019 IEEE. P ersonal use of this material is p ermitted. P ermission from IEEE m ust b e obtained for all other uses, in an y curren t or future media, including reprinting/republishing this material for advertising or promotional purp oses, creating new collectiv e w orks, for resale or redistribution to serv ers or lists, or reuse of any cop yrigh ted component of this w ork in other w orks. PEDESTRIAN DEAD-RECKONING ALGORITHMS F OR DUAL F OOT-MOUNTED INER TIAL SENSORS I. A. Chistiak o v Huawei T e chnolo gies Co. Ltd. Mosco w, Russia c hist y ak o v.iv an@y aho o.com A. A. Nikulin Huawei T e chnolo gies Co. Ltd. Mosco w, Russia nikulin.alexey@h ua w ei.com I. B. Gartseev Huawei T e chnolo gies Co. Ltd. Mosco w, Russia gartseev.ilia@h ua w ei.com A bstr act —This w ork prop oses algorithms for re- construction of closed-lo op p edestrian tra jectories based on t w o fo ot-mounted inertial measurement units (IMU). The first prop osed algorithm allo ws calculation of a tra jectory using measuremen ts from only one IMU. The second algorithm uses data 1 from b oth fo ot- moun ted IMUs sim ultaneously . Both algorithms are based on the Kalman filter and the assumption that while a fo ot is on the ground its v elo cit y is supp osed to b e zero. T w o methods for comparing the obtained tra- jectories are prop osed, adv antages and disadv an tages of eac h metho d are indicated and a wa y to optimize the computation time is presen ted. In addition, a metho d is prop osed for constructing one generalized tra jectory of human motion based on the tra jectories of each leg. Index T erms —p edestrian na vigation, indoor navi- gation, inertial na vigation, fo ot-mounted na vigation, p edestrian dead-reck oning, IMU, inertial measurement unit, dual fo ot-moun ted INS, ZUPT-aided INS I. Intr oduction Existing metho ds for recov ering p edestrian tra jectories using inertial sensor data mak e it p ossible to significan tly correct the constructed tra jectories using filtering algo- rithms [1]–[4]. At the same time, accuracy of the results is highly dep endent on the attachmen t p oint of the mea- surmen t units [5]. In this work, MPU-9250 sensors [6] are moun ted in the area of foot elev ation (Fig. 1). This metho d of attac hmen t allows using the assumption of zero speed of devices at the moments of con tact of a fo ot with a surface, but it do es not provide an y additional information about orien tation of devices in space. Thus, the lac k of accuracy of sensors leads to a significant deviation of calculated tra jectories from the real ones. Since inertial metho ds give acceptable results only on short tra jectories (up to three minutes), an additional restriction is introduced to solv e the problem: only closed tra jectories are considered. Due to this assumption it is p ossible to use coincidence of the initial and final p oints and to significantly influence tra jectories constructed dur- ing the post-pro cessing of data. Since a new measurement 1 The data used in the article are av ailable for downloading at http://gartseev.ru/projects/mkins2019. is reflected only in the section of the tra jectory correspond- ing to the last step, w e prop ose a smo othing algorithm that corrects the en tire calculated curv e. Figure 1. Placement of measuremen t units. Both the p osition and the orientation of the devices can also b e corrected if there are data from tw o differen t IMUs fixed resp ectiv ely on the righ t and left legs. The adv an tage of using m ultiple sensors has b een sho wn, for example, in reference [7]. In this work, we use the additional as- sumption that p ositions of the sensors in space cannot significan tly differ from eac h other. An algorithm was de- v elop ed that allo ws using all of the data men tioned ab ov e (information ab out the initial and final positions, pseudo- observ ations of velocity , information about p osition of the other leg) to calculate smo oth tra jectories of both legs and to construct one generalized tra jectory afterw ards. The presented algorithms w ere tested during exp eri- men ts with lengths of one to 15 minutes, whic h to ok place indoors on horizon tal surfaces. In all experiments the devices w ere moun ted in the area of fo ot elev ation. I I. System description A. Non-line ar system The dynamic system that describes the motion of a foot- moun ted IMU is non-linear and has the follo wing form:        p n = p n − 1 + v n − 1 dt, v n = v n − 1 +  C T n f n + g  dt, C n = R n C n − 1 , (1) where n is a time index, dt is a time difference b et w een consecutiv e measurements, p n ∈ R 3 is the p osition of an IMU in the na vigation frame, v n ∈ R 3 is the velocity vec- tor, f n ∈ R 3 and w n ∈ R 3 are resp ectively the sp ecific force and angular rate in the bo dy frame, g is the gra vity force, C n ∈ R 3 × 3 is the orientation matrix of the b ody frame relativ e to the na vigation frame and R n = R n ( w n dt ) is the rotation matrix. As far as eac h matrix C n is associated with a set of roll, pitch and y aw angles θ n ∈ R 3 , the state vector x n = [ p n v n θ n ] T is considered. Ho w ev er, there are other w a ys of represen ting sys- tem (1), for example, using quaternions [1]. In this case, the orientation of the device c hanges in accordance with the follo wing equation: q n =  cos ( α n ) I 4 + 1 α n sin ( α n ) Ω n  q n − 1 , where α n = 0 . 5 · k w n k dt , I 4 ∈ R 4 is the iden tit y matrix, Ω n = dt 2     0 w 3 n − w 2 n w 1 n − w 3 n 0 w 1 n w 2 n w 2 n − w 1 n 0 w 3 n − w 1 n − w 2 n − w 3 n 0     , and the v elo cit y v ector c hanges according to v n = v n − 1 + ( q n − 1 f n q ∗ n − 1 + g ) dt. B. Line arize d system In the p ost-pro cessing, we also assume the presence of random measurement errors of accelerometers and gyro- scop es ( ˜ w n and ˜ f n are the measuremen ts): w n = ˜ w n + δ w n , f n = ˜ f n + δ f n . Let a b e an arbitrary vector in R 3 . W e use the following notation: ˆ a =   0 a 3 − a 2 − a 3 0 a 1 a 2 − a 1 0   ∈ R 3 × 3 . (2) Let C = C ( t ) be the real orientation matrix of an IMU and ˜ C = ˜ C ( t ) be the calculated orientation matrix. Since they do not coincide (e.g. due to the initial orientation error), then C T = M ˜ C T , where M is some orthogonal matrix. If the difference betw een C and ˜ C is small, then matrix M ma y be describ ed b y a small vector of rotation β ∈ R 3 . So, C T ≈  I + ˆ β  ˜ C T . (3) Differen tiating b oth sides of the equalit y , w e get: ∂ ∂ t C T =  ∂ ∂ t ˆ β  ˜ C T +  I + ˆ β  ∂ ∂ t ˜ C T . (4) Since the matrices C and ˜ C satisfy the P oisson form ula ∂ ∂ t C =  ˆ ˜ w + δ ˆ w  С , ∂ ∂ t ˜ C = ˆ ˜ w ˜ С , then (4) can b e rewritten using (3): ∂ ∂ t ˆ β = − C T · δ ˆ w · ˜ C , (5) ∂ ∂ t β ≈ − ˜ C T · δ w . (6) Since the equality ˆ ab = − ˆ ba is correct for any vectors a ∈ R 3 , b ∈ R 3 , w e get the follo wing equalit y for v : ∂ ∂ t v = С T f + g = ( I + ˆ β ) ˜ С T f + g = ˆ β  ˜ С T f  + +  ˜ С T f + g  = −  d ˜ С T f  β +  ˜ С T f + g  ≈ ≈ −  d ˜ С T ˜ f  β +  ˜ С T ˜ f + g  + ˜ C T δ f . (7) Using (6) and (7), it is p ossible to change the current set of v ariables to x n = [ p n v n β n ] T and to consider the linear system with matrix F n : F n =    I 3 × 3 I 3 × 3 dt O 3 × 3 O 3 × 3 I 3 × 3 − \ ˜ С T n ˜ f n dt O 3 × 3 O 3 × 3 I 3 × 3    . (8) I I I. Algorithm for one IMU The algorithm is based on the use of function T that detects a stationary fo ot p osition when w alking [2]. W e assume that position is stationary if the v alue of T is less than some giv en constan t: T  n ˜ f i , ˜ w i o W n  < γ , (9) where W n is a time windo w of the length N samples cen tered around t n , γ > 0 is a zero-velocity detection threshold. When the system is stationary , the velocity co ordinates of a calculated vector x n can b e treated as a pseudo- measuremen t of the velocity estimation error (since the real velocity is supposed to b e zero). Hence, we get obser- v ation v n = 0 with observ ation matrix H =  O 3 × 3 I 3 × 3 O 3 × 3  ∈ R 3 × 9 . The corresp onding algorithm [1] is shown in Algo- rithm 1. The following notation is used: F n ∈ R 9 × 9 is the matrix of linearized system (8), P n ∈ R 9 × 9 is the error co v ariance matrix, Q ∈ R 6 × 6 is the cov ariance matrix of the measuremen t noise, R ∈ R 3 × 3 is the cov ariance matrix for the noises of the observ ations, G n is the pro cess noise matrix: G n =    O 3 × 3 O 3 × 3 ˜ C T n dt O 3 × 3 O 3 × 3 − ˜ C T n dt    ∈ R 9 × 6 , ˜ C n ∈ R 3 × 3 is a calculated estimation of orientation matrix and the function f mech corresp onds to form ulas (1). W e also assume that at the b eginning and at the end of each exp eriment measurement units are retained in a Algorithm 1: Kalman filter. Initialization: ˜ x 0 ← E [ x 0 ] , P 0 ← cov ( x 0 ) for n = 1 to N do /* Time up date */ ˜ x n ← f mech ( ˜ x n − 1 , ˜ f n , ˜ w n ) P n ← F n P n − 1 F T n + G n QG T n /* Measuremen t up date */ if T  { ˜ f i , ˜ w i } W n  < γ then K n ← P n H T  HP n H T + R  − 1 δ ˜ x n ← K n ˜ v n P n ← ( I 9 × 9 − K n H ) P n /* Comp ensate in ternal states */  ˜ p n ˜ v n  ←  ˜ p n ˜ v n  +  δ ˜ p n δ ˜ v n  ˜ C n ← ( I 3 × 3 + ˆ β n ) ˜ C n δ ˜ x n ← 0 , β n ← 0 resting state for a few seconds. Thus, in these moments it is p ossible to use additional information ab out the initial p osition p n = 0 (without loss of generality w e consider it to b e zero). Then the observ ation matrix is H =  I 3 × 3 O 3 × 3 O 3 × 3 O 3 × 3 I 3 × 3 O 3 × 3  ∈ R 6 × 9 . In order for the added p osition adjustment to tak e place not only at the end of the mov emen t, but to reflect on the whole tra jectory , a smo othing R TS filter (Algorithm 2), also describ ed in [1], is used. Smoothing also allows to get rid of the discon tin uities that occur during the correction at the end of each step. Ho w ev er, since the system of equations (1) is non-linear, the errors of angle estimation increase quickly . That leads to a discrepancy b etw een equations (1) and linear approximation (8). T o solv e this problem, it is suggested to correct the angle v alues at the forward Kalman filter stage. The pseudo-co de of the prop osed algorithm is giv en in Algorithm 3. Algorithm 2: R TS smo othing. for n = N − 1 to 1 do A n ← P n | n F T n P − 1 n +1 | n ˜ x n | N ← ˜ x n | n + A n  ˜ x n +1 | N − ˜ x n +1 | n  P n | N ← P n | n + A n  P n +1 | N − P n +1 | n  A T n The difference betw een tw o methods is shown in Fig. 2. Red color path corresp onds to the tra jectory that was cal- culated without preliminary angle correction. Y ellow color path corresp onds to the tra jectory that was calculated with the prop osed metho d. It is kno wn that the first and the last segments of the real tra jectory passed along the same line, ho w ev er, a few meters difference ma y b e noticed in the first case. Algorithm 3: Algorithm that tak es closureness into accoun t. Initialization: ˜ x 0 ← E [ x 0 ] , δ ˜ x 0 ← 0 , P 0 ← cov ( x 0 ) /* F orward Kalman filter stage */ for n = 2 to N do /* Time up date */ ˜ x n ← f mech ( ˜ x n − 1 , ˜ f n , ˜ w n ) δ ˜ x n | n − 1 ← F n δ ˜ x n − 1 | n − 1 P n | n − 1 ← F n P n − 1 | n − 1 F T n + G n QG T n /* Measuremen t up date */ if T  { ˜ f i , ˜ w i } W n  < γ then if standstill( n ) = true then H ←  I 3 × 3 O 3 × 3 O 3 × 3 O 3 × 3 I 3 × 3 O 3 × 3  K n ← P n | n − 1 H T  HP n | n − 1 H T + R 0  − 1 δ x n | n ← δ x n | n − 1 − K n  δ p n | n − 1 − p n δ v n | n − 1 − v n  else H ←  O 3 × 3 I 3 × 3 O 3 × 3  K n ← P n | n − 1 H T  HP n | n − 1 H T + R 00  − 1 δ x n | n ← δ x n | n − 1 − K n ( δ v n | n − 1 − v n ) P n | n ← ( I 9 × 9 − K n H ) P n | n − 1 /* Comp ensate in ternal angle states */ ˜ C n ← ( I 3 × 3 + ˆ β n ) ˜ C n δ ˜ θ n ← 0 , β n ← 0 /* Smo othing */ for n = N − 1 to 1 do A n ← P n | n F T P − 1 n +1 | n δ ˜ x n | N ← δ ˜ x n | n + A n ( δ ˜ x n +1 | N − δ ˜ x n +1 | n ) P n | N ← P n | n + A n ( P n +1 | N − P n +1 | n ) A T n /* Comp ensate in ternal states */ for n = 1 to N do ˜ x n ← ˜ x n + δ ˜ x n | N δ ˜ x n ← 0 Figure 2. Comparison of tra jectories with preliminary correction of angles and without it. A. Metrics of the p ath similarity T o assess the qualit y of results of the proposed algo- rithm, it is p ossible to conduct an experiment in whic h IMUs are fixed on b oth legs of a p erson and then compare t w o reconstructed tra jectories b etw een themselv es. T o do this, a metric is needed that is capable of comparing t w o curv es. The same metric is needed in order to put optimally one tra jectory ov er another (since the initial orien tations of IMUs do not coincide). In this case, the metric should take into account the following features: generally speaking, the durations of the experiment for the left and right legs are different (the sensors are not alw a ys switc hed on at the same time), IMUs do not w ork sync hronously and some measurements may b e omitted. T wo metrics that satisfy the given requirements were in v estigated: discrete F r´ ec het distance and DTW-metric. 1) Discr ete F r´ echet distanc e: Due to the discrete na- ture of measuremen ts, eac h calculated tra jectory may b e treated as a p olygonal curve C : [1 , n ] → R 3 , where n is the num b er of measuremen ts tak en during an exp eriment. The polygonal curv e is uniquely represen ted with a set σ ( C ) = ( C (0) , C (1) , . . . , C ( n )) of segment ends, so that for an y λ ∈ [0 , 1] the following equalit y is fulfilled: C ( i + λ ) = (1 − λ ) C ( i ) + λC ( i + 1) , i = 1 , n. Let C = ( ν 1 , . . . , ν n ) and R = ( µ 1 , . . . , µ m ) b e tw o p olygonal curves and let L denote the sequence of distinct pairs ( ν a 1 , µ b 1 ) , . . . , ( ν a s , µ b s ) , where a 1 = 1 , b 1 = 1 , a s = n , b s = m and either a i = a i − 1 or a i = a i − 1 + 1 is correct for any i = 2 , n , as w ell as b j = b j − 1 or b j = b j − 1 +1 is correct for any j = 2 , m . The length k L k of coupling L is defined as the length of the longest link in L : k L k = max 1 ≤ i ≤ s d ( ν a i , µ b i ) , where d is some metric of the R 3 space (e.g. the Euclidean metric). Discrete F r´ ec het distance is defined to b e F ( C, R ) = min L {k L k} . The algorithm dev elop ed by Eiter and Manilla in [8] allo ws to calculate the discrete F rec het distance o v er time O ( nm ) using dynamic programming metho d, where m and n are the lengths of p olygonal curv es. 2) Dynamic time warping: The dynamic time w arping algorithm (DTW) is a metho d for calculating an optimal matc h b etw een tw o given sequences with certain restric- tions: • Ev ery index from the first sequence m ust be matched with one or more indices from the other sequence, and vice v ersa. • The first index from the first sequence must b e matc hed with the first index from the other sequence (but it do es not ha v e to b e its only matc h). • The last index from the first sequence must b e matc hed with the last index from the other sequence (but it do es not ha v e to b e its only matc h). • The mapping of the indices of the first sequence to the indices of the other sequence must b e monotonically increasing, i.e. if j > i are indices from the first sequence, then there m ust not b e t wo indices l > k in the other sequence suc h that index i is matched with index l and index j is matched with index k , and vice v ersa. The optimal match is denoted b y the match that satisfies all the restrictions and that has the minimal sum of distances b et w een the corresp onding p oin ts. Let Q = ( q 1 , q 2 , . . . , q n ) and C = ( c 1 , c 2 , . . . , c m ) be t w o time series. The trivial DTW algorithm consists of the follo wing steps: 1. Calculation of suc h a matrix d ∈ R n × m that eac h elemen t d i,j of d is equal to the distance b etw een q i and c j . 2. Calculation of a transformation matrix D . Eac h ma- trix elemen t is defined as D i,j =                d 1 , 1 , if i = 1 , j = 1 , d i,j + D i − 1 ,j , if i 6 = 1 , j = 1 , d i,j + D i,j − 1 , if i = 1 , j 6 = 1 , d i,j + min { D i − 1 ,j , D i − 1 ,j − 1 , D i,j − 1 } , if i 6 = 1 , j 6 = 1 . 3. Calculation of the optimal transformation path de- noted W = ( w 1 , w 2 , . . . , w k ) , whic h is a set of adjacen t elemen ts of matrix D and whic h establishes a correspondence b etw een Q and C . Here, k is a n um b er of elements in the sequence W . The transformation path is c hosen in suc h a wa y that the sum of distances betw een the corresponding p oin ts is minimal. 4. The DTW distance is calculated as follo ws: D T W ( Q, C ) = 1 k k X i =1 d ( w i ) . The time complexity of the presented algorithm is O ( nm ) . How ev er, it ma y b e reduced by using one of the algorithm mo difications [9]. B. Comp arison of the metrics F or comparison of the metrics, tra jectories of different legs were analyzed and pairs of corresp onding points w ere selected. It is shown in Fig. 3 that the DTW algorithm selects pairs of p oints more accurately (some matches are marked in red). F r ´ echet metric giv es worse results in cases when the corresp onding parts of the curv es are lo cated at a large angle to each other. Therefore, further prop osed algorithms use DTW-metric. Nev ertheless, there is an example (see Fig. 4) when b oth algorithms fail to detect correspondence correctly . (a) DTW-metric (b) F r´ echet metric Figure 3. Comparison of the metrics Incorrect mapping takes place when turning p oints of tra jectories are at a significan t distance from eac h other. In some cases p oints that are far apart in time can b e mapp ed by an algorithm although such mapping is undesirable. Th us, the results of the algorithms can be impro v ed by using an additional parameter w — the maxim um allo w ed difference betw een the indices of the corresp onding points (index is the ordinal num b er of a measuremen t in the current tra jectory). It is desirable to choose a v alue w whic h do es not exceed the av erage n um b er of measurements during one step. The result of using this approach is shown in Fig. 5. If tw o sequences ha v e the same num ber of p oints and w = 1 , then b oth metho ds matc h p oin ts with equal sequence n um b er. It should b e noted that the most simple algorithms for calculation of the mentioned metrics use a metho d of dynamic programming, in which distances in all pairs of p oints are calculated. How ev er, the approach with an (a) DTW-metric (b) F r´ echet metric Figure 4. Comparison of metrics without the use of additional constraint. additional constraint reduces the algorithm time com- plexit y as well as the space complexit y from O ( nm ) to O ( w · max( n, m )) . C. Path b e am aver aging The results of eac h introduced algorithm also include a sequence of index pairs that determines the mapping of one curve to another. If we tak e the middle p oint of ev ery segment joining a pair of “similar” p oints, we will obtain a certain new curve. An analysis of such a metho d of creating the “a v erage” tra jectory using the DTW algorithm is presented in [10]. Ho w ev er, that metho d of av eraging loses information ab out time of measurements since the p oints mapp ed to each other can b e obtained at differen t momen ts. If that information is important, p oints with equal sequence n um b ers should b e matc hed. (a) DTW-metric (b) F r´ echet metric Figure 5. Comparison of metrics with the use of additional constraint. IV. Algorithm for two IMUs In the middle of a step the p osition of a mo ving fo ot is supposed to be approximately equal to a position of the otherot fo ot. A ccording to that idea, the additional observ ations ma y b e passed to the Kalman filter. Thus, it is suggested to calculate tra jectories of differen t legs side b y side. The algorithm consists of the follo wing steps: 1) Both tra jectories are calculated separately using the filter described in Algorithm 3. Then tra jectories are o v erlapp ed to determine the initial difference b et w een the y a w angles. F or that purpose, the DTW distance is minimized b y selecting the initial angles. 2) A leg that makes the first step is determined. It is supp osed that the first step is made from the initial standstill p osition and is shorter than the first step of another leg. Algorithm 4: Algorithm that uses data from b oth IMUs. cur ← 2 // begin computation with the second leg p os ← ˜ x 1 k 1 // curren t p osition of the first leg while k 1 < N 1 || k 2 < N 2 do /* select data for the curren t leg */ if cur = 1 then ˜ x n − 1 ← ˜ x 1 n − 1 , k ← k 1 , ˜ f n ← ˜ f 1 n , ˜ w n ← ˜ w 1 n else ˜ x n − 1 ← ˜ x 2 n − 1 , k ← k 2 , ˜ f n ← ˜ f 2 n , ˜ w n ← ˜ w 2 n /* pro cess mo v emen t of the curren t leg */ step _ len = count_step_length( k , { ˜ f i , ˜ w i } ) for n = k + 1 to k + step _ len do [ ˜ x n , δ ˜ x n | n − 1 , P n | n − 1 ] ← predict( ˜ x n − 1 , ˜ f n , ˜ w n ) if n = k + step _ len/ 2 then /* correction step of Kalman filter using curren t p osition of another leg */ H ←  I 2 × 2 O 2 × 7  K n ← P n | n − 1 H T  HP n | n − 1 H T + R 000  − 1 /* select 2 co ordinates */ ˜ x 1 , 2 n ← H ˜ x n , δ ˜ x 1 , 2 n | n − 1 ← H δ ˜ x n | n − 1 δ ˜ x n | n ← δ ˜ x n | n − 1 − K n ( p os + δ ˜ x 1 , 2 n | n − 1 − ˜ x 1 , 2 n ) P n | n ← ( I 9 × 9 − K n H ) P n | n − 1 n ← k + step _ len + 1 /* stationary p osition of the curren t leg */ while T  { ˜ f i , ˜ w i } W n  < γ do [ ˜ x n , δ ˜ x n | n − 1 , P n | n − 1 ] ← predict( ˜ x n − 1 , ˜ f n , ˜ w n ) if standstill( n ) = true then H ←  I 3 × 3 O 3 × 3 O 3 × 3 O 3 × 3 I 3 × 3 O 3 × 3  , R ← R 0 else H ←  O 3 × 3 I 3 × 3 O 3 × 3  , R ← R 00 [ δ ˜ x n | n , P n | n ] ← correct( H , R , P n | n − 1 , ˜ x n , δ ˜ x n | n − 1 ) n ← n + 1 /* sa v e curren t p osition for the follo wing observ ation */ p os ← ˜ x 1 , 2 n /* sa v e up dated data */ if cur = 1 then ˜ x 1 n ← ˜ x n , δ ˜ x 1 n | n ← δ ˜ x n | n , P 1 n | n ← P n | n , k 1 ← n cur ← 2 else ˜ x 2 n ← ˜ x n , δ ˜ x 2 n | n ← δ ˜ x n | n , P 2 n | n ← P n | n , k 2 ← n cur ← 1 3) T wo “fused” tra jectories are сomputed using data from b oth sensors. 4) R TS smo othing is applied to b oth calculated tra jec- tories (see Algorithm 3). 5) The obtained tra jectories are optimally ov erlapped once again using brute-force angle search. Then the a v erage tra jectory is calculated with use of the DTW- algorithm. Step 3 is describ ed in detail in Algorithm 4 (v ariables with upp er index 1 corresp ond to a leg (IMU) that makes the first step; upp er index 2 stands for the other IMU). F unctions predict and correct stand for prediction and correction steps of the Kalman filter (see Algorithm 3). V. Resul ts Fig. 6-7 contain tra jectories obtained with an algorithm whic h do es not take the final observ ation into accoun t (nev ertheless, the smo othing is applied). Also, tra jectories reconstructed with Algorithms 3 and 4 are presented there (tra jectories of left and right legs are marked in y ello w and cy an colors resp ectively; red color stands for the final generalized tra jectories). The DTW-distances b et w een corresp onding tra jectories of left and right legs are indicated as w ell. T ra jectories in Fig. 7 correspond to a double pass bac k and forth along a curv ed corridor. N Alg. [1] Alg. 3 Alg. 4 Duration, s 1 4.460 0.292 0.252 93,5 2 20.430 1.317 0.723 216.7 3 14.981 3.206 0.939 245.5 4 16.565 1.437 1.042 250,9 5 6.690 1.951 1.491 332.8 6 45.630 17.917 1.546 630.1 T able I. Results of the presented algorithms. The figures illustrate the proximit y of resconstructed tra jectories of the INS attached to different legs of a p erson. Moreo v er, the presented tra jectories reproduce the metric characteristics of ro oms in which the exp eriments w ere conducted and it further confirms the correctness of the restoration, allowing the use of the proposed al- gorithms when more reliable reference information is not a v ailable. T able I shows the duration times of several exp eriments and v alues of DTW metric, obtained as results of the presen ted algorithms. T ra jectories 1 and 2 corresp ond to a single pass back and forth along a straight line, tra jectory 3 was obtained as a result of a double pass around the p erimeter of the ro om. The tra jectories 4 and 5 corresp ond to a single pass bac k and forth along a L-shap ed corridor, tra jectory 6 corresponds to a double pass along it. The v alues of DTW metric do not give a direct answ er to the question ab out the metric difference of the recon- structed and real tra jectories, but they allow us to state that the prop osed algorithms reconstruct real tra jectories with significantly reduced error compared to tra jectories reconstructed according to a single INS. (a) Alg. [1], d = 2 . 949 . (b) Alg. 3, d = 0 . 241 . (c) Alg. 4, d = 0 . 283 . Figure 6. Comparison of algorithms. (a) Alg. [1], d = 25 . 862 . (b) Alg. 3, d = 13 . 883 . (c) Alg. 4, d = 1 . 354 . Figure 7. Comparison of algorithms. VI. Conclusion The pap er contains description of the algorithms for reconstruction of close-lo op tra jectories based on infor- mation ab out acceleration and angular velocity (for only one IMU and for IMUs on b oth legs). The corresp onding pseudo co de is presented. T wo wa ys of comparing the obtained tra jectories are prop osed, their adv antages and disadv antages are considered, and a metho d of optimizing the computation time is sp ecified. In addition, a metho d for constructing a single combined tra jectory based on measuremen ts from the IMUs installed on eac h of tw o feet is prop osed. Both prop osed algorithms were tested on real data and thus demonstrated their efficiency as a to ol for obtaining reference paths when more accurate reference information is not a v ailable. References [1] Colomar, D. S., Nilsson, J.-O., and H¨ andel, P . , Smo othing for ZUPT-aided INSs, 2012 In ternational Conference on Indoor Positioning and Indoor Navigation (IPIN), Sydney , IEEE, 2012, pp. 1-5. [2] Nilsson, J.-O. , Sk og, I., H¨ andel, P ., and Hari, K. V. S. , F oot-mounted INS for everybo dy — an op en-source embedd ed implementation, Pro ceedings of the 2012 I EEE/ION P osition, Location and Navigation Symp osium, Myrtle Beach, IEEE, 2012, pp. 140-145. [3] Nilsson, J.-O., Gupta, A. K., and H¨ andel, P . , F o ot- mounted inertial navigation made easy , 2014 International Con- ference on Indo or Positioning and Indoor Navigation (IPIN), Busan, IEEE, 2014, pp. 24-29. [4] W ang, Y., Chernyshoff, A., and Shkel, A. M. , Error Analysis of ZUPT-Aided Pedestrian Inertial Navigation, 2018 International Conference on Indo or P ositioning and Indoor Nav- igation (IPIN), Nantes, IEEE, 2018. [5] Tjhai, C., and Okeefe, K. , Comparing Heading Estimates from Multiple W earable Inertial and Magnetic Sensors Moun ted on Low er Limbs, 2018 In ternational Conference on Indoor Po- sitioning and Indo or Navigation (IPIN), Nantes, IEEE, 2018. [6] MPU-9250 Pro duct Sp ecification, Revision 1.1, https://www.in vensense.com/wp- conten t/uploads/2015/02/PS-MPU-9250A-01-v1.1.p df. [7] Mik o v, A. G., and Galov, A. S. , Data pro cessing algorithms for MEMS based multi-component inertial measuremen t unit for indoor na vigation, 2017 24th Saint P etersburg International Conference on Integrated Navigation Systems (ICINS), St. Pe- tersburg, IEEE, 2017, pp. 1-7. [8] Eiter, T., and Mannila, H. , Computing discrete F r´ echet distance, T echnical Report CD-TR 94/64, Christian Doppler Laboratory for Exp ert Systems, Vienna, 1994. [9] M ¨ uller, M. , “Dynamic Time W arping” in Information Retriev al for Music and Motion, Springer, 2007, pp. 69-84. [10] V aughana, N., and Gabrysa, B. , Comparing and combining time series tra jectories using Dynamic Time W arping, Procedia Computer Science, 2016, vol. 96, pp. 465-474.