Improved Pedestrian Dead Reckoning Based on a Robust Adaptive Kalman Filter for Indoor Inertial Location System

sensors

Article Improved Pedestrian Dead Reckoning Based on a Robust Adaptive Kalman Filter for Indoor Inertial Location System

Qigao Fan 1,†, Hai Zhang 1,*,†, Peng Pan 2,†, Xiangpeng Zhuang 1, Jie Jia 1, Pengsong Zhang 1, Zhengqing Zhao 1, Gaowen Zhu 1 and Yuanyuan Tang 1 1 Internet of Things Engineering, Jiangnan University, Wuxi 214000, China; [email protected] (Q.F.); [email protected] (X.Z.); [email protected] (J.J.); [email protected] (P.Z.); [email protected] (Z.Z.); [email protected] (G.Z.); [email protected] (Y.T.) 2 Department of Mechanical Engineering, McGill University, Montreal, QC H3A 0G4, Canada; [email protected] * Correspondence: [email protected]; Tel.: +86-188-0059-0310 † These authors contributed equally to this work.

Received: 23 November 2018; Accepted: 8 January 2019; Published: 12 January 2019

Abstract: Pedestrian dead reckoning (PDR) systems based on a microelectromechanical-inertial measurement unit (MEMS-IMU) providing advantages of full autonomy and strong anti-jamming performance are becoming a feasible choice for pedestrian indoor positioning. In order to realize the accurate positioning of pedestrians in a closed environment, an improved pedestrian dead reckoning algorithm, mainly including improved step estimation and heading estimation, is proposed in this paper. Firstly, the original signal is preprocessed using the wavelet denoising algorithm. Then, the multi-threshold method is proposed to ameliorate the step estimation algorithm. For heading estimation suffering from accumulated error and outliers, robust adaptive Kalman ﬁlter (RAKF) algorithm is proposed in this paper, and combined with complementary ﬁlter to improve positioning accuracy. Finally, an experimental platform with inertial sensors as the core is constructed. Experimental results show that positioning error is less than 2.5% of the total distance, which is ideal for accurate positioning of pedestrians in enclosed environment.

Keywords: indoor inertial positioning; MEMS-IMU; improved pedestrian dead reckoning; robust adaptive Kalman ﬁlter

1. Introduction Indoor positioning is a technology for positioning in an indoor environment, which shows great prospects in industries such as emergency rescue, logistics, wireless games and shopping malls [1–3]. The Global Navigation Satellite System, such as the Global Positioning System (GPS), BeiDou Navigation Satellite System (BDS), and Global Navigation Satellite System (GLONASS), can provide absolute accurate positioning, but its performance in some enclosed environments, such as tunnels and mines, is not satisfied; as the Global Navigation Satellite System may suffer heavily from satellite signal blockage [4]. Therefore, how to obtain accurate position information of indoor objects has attracted wide attention. At present, indoor positioning methods have been proposed, including Ultra Wideband (UWB) [5], ZigBee [6], Wi-Fi [7], Bluetooth [8], Radio Frequency Identification (RFID) [9], and visual positioning [10]. However, performances of these positioning methods are easily affected by external environment. UWB positioning is susceptible to non-line-of-sight interference such as occlusion, external collision and strong magnetic field interference [11]. Wi-Fi positioning is susceptible

Sensors 2019, 19, 294; doi:10.3390/s19020294 www.mdpi.com/journal/sensors Sensors 2019, 19, 294 2 of 21 to interference from other signals and consumes a lot of power. In addition, ZigBee and Bluetooth positioning are not suitable for complex environments, in which the performance is not stable. RFID positioning does not have communication capabilities, and the anti-interference ability is poor. Visual positioning has poor real-time performance and is limited by light conditions, which cannot work in dark environments [12]. With the continual miniaturization of Micro-Electro-Mechanical Systems (MEMS) in recent years, the MEMS-IMU has the advantages of simple structure and portability. At the same time, inertial measurement is completely autonomous, suitable for all weather, free from external environment interference, and has no signal loss, so MEMS-IMU-based inertial navigation technology is very suitable for indoor positioning [13,14]. Pedestrian dead reckoning (PDR) is an effective technique for indoor positioning of pedestrians [15–17]. By measuring the movement information of pedestrians in real time, the pedestrian’s step, step length, and heading angle are identified to calculate the position of the pedestrian. The indoor positioning based on MEMS-IMU is widely used because of its advantages such as portability, simple structure, and strong anti-interference performance. However, errors of the Inertial positioning system accumulated over time, which greatly affects the positioning accuracy [18]. Domestic and foreign experts have proposed combined positioning system to suppress cumulative errors through auxiliary positioning techniques, such as INS/GNSS [19], INS/RFID [20], and INS/UWB positioning [21]. The combined positioning technology can improve the positioning accuracy compared with the single inertial positioning technology, but its external requirements are more demanding, which greatly limits the scope of use of the combined positioning system. In scenes with harsh external environments, such as the disaster scene of mines or tunnels, the combined positioning system is difficult to build and wastes valuable rescue time. The single inertial positioning technology is extremely suitable because of its characteristics, so the study of single inertial positioning system has high practical value. In the framework of pedestrian dead reckoning, the step estimation is very important. The key to correctly estimate step frequency is to avoid acceleration warpage and sensor noise, which may lead to incorrect step estimation. Noises usually exit in the signal obtained by MEMS-IMU, which result in the poor positioning result. To improve accuracy of positioning, it is necessary to preprocess the signal. Low pass filter is typically used to smooth the data, but it is difficult to determine the appropriate cutoff frequency [22]. The authors in [23] analyze error noise term by Allan variance method, and optimized Kalman filter is designed to denoise the signal according to noise correlation, but the amount of calculation is large. At present, the step estimation algorithm usually includes Peak detection [24–26], Zero crossings [27] and fast Fourier transform [28], where the signals of accelerometer are generally used. In these algorithms, the placement of the IMU defaults to one of the accelerometer axes being always vertical [29], which greatly affects the shape of the accelerometer signal and the effectiveness of the algorithm. Step length estimation plays an important role in the pedestrian dead reckoning. For different individuals, the step length of different individuals varies. Even for the same person, step length will change due to different states of motion. Usually, there are two kinds of models for estimating the step length: static model and dynamic model. It is assumed that in the static model, the step length is a fixed value, which is obviously inaccurate. In comparison, the dynamic model considers that any effective stride has a different step length, which is affected by the different individuals or different states of motion. The step dynamic model includes step-frequency linear model [30], nonlinear model [31], and adaptive learning step length model. A nonlinear model is used in [32] to accommodate the difference of step length between different pedestrians, which highly stable and moderately complex compared to other dynamic step models. Heading estimation is the core of the pedestrian dead reckoning algorithm. Calculating the heading angle using the magnetometer has the advantages of simple structure, low power consumption, and no cumulative errors [33]. However, magnetometers are susceptible to external environmental disturbances, especially in mines or garages, where there are many ferromagnetic Sensors 2019, 19, 294 3 of 21 materials. There are serious cumulative errors when using the gyroscope alone, especially at corners. The longer the time is, the larger the cumulative error will be [34]. In order to improve the heading accuracy of the inertial measurement unit, fusion filtering algorithms are proposed, such as Kalman filtering [35] and complementary filtering [36]. The authors in [37] use an extended Kalman filter based on two-levelSensors 2019, 19 quaternions, x FOR PEER REVIEW to fuse signals of acceleration, angular velocity and magnetic to3 of minimize 22 thefiltering cumulative [35] and error, complementary but ignores the filtering change [36]. in The noise authors characteristics. in [37] use Therefore,an extende thed Kalman adaptive filter filtering algorithmbased on is proposed.two-level quaternions In the literature to fuse [38 signals], the fuzzy of acceleration, adaptive Kalman angular velocity filter is used and magnetic to fuse the to signals of gyroscopeminimize theand cumulative accelerometer, error, but but it is ignores limited the by the change precision in noise of the characteristics. fuzzy control, Therefore, and the calculation the amountadaptive is large. filtering Sage–Husa algorithm adaptiveis proposed. filtering In the [ 39literature] can correct [38], the the fuzzy statistical adaptive characteristics Kalman filter of is process noiseused and to fuse measurement the signals of noise gyroscope in real and time accelerometer, to improve but the it filtering is limited accuracy, by the precision but it isof difficultthe fuzzy to give accuratecontrol, noise and characteristics the calculation and amount the calculation is large. Sage amount–Husa is adaptive large. In filtering comparison, [39] can the correct complementary the fusionstatistical filter characteristics [40] complements of process the noise signals and ofmeasurement accelerometer, noise in magnetometer real time to improve and gyroscopethe filtering with a accuracy, but it is difficult to give accurate noise characteristics and the calculation amount is large. small amount of calculation, but high precision. The paper is structured as follows: Section2 briefly In comparison, the complementary fusion filter [40] complements the signals of accelerometer, introduces the indoor positioning model based on the PDR algorithm firstly, and then the wavelet magnetometer and gyroscope with a small amount of calculation, but high precision. The paper is decompositionstructured as isfollows: used to Section preprocess 2 briefly the introduces original signal, the indoor finally positioning the proposed model improved based on PDRthe PDR algorithm is discussedalgorithm infirs detail,tly, and including then the wavelet improved decomposition step estimation, is used to length preprocess estimation, the original and headingsignal, finally estimation basedthe proposed on fusion improved filtering. PDR In algorithm Section3, is several discussed experiments in detail, including are performed improved and step the estimation, experimental resultslength are estimation, analyzed and based heading on established estimation experimentalbased on fusion platform. filtering. In Some Section final 3, several remakes experiments in Section 4 end theare paper. performed and the experimental results are analyzed based on established experimental platform. Some final remakes in Section 4 end the paper. 2. Materials and Methods 2. Materials and Methods 2.1. System Modeling 2.1. System Modeling 2.1.1. Pedestrian Dead Reckoning 2.1.1. Pedestrian Dead Reckoning Pedestrian dead reckoning algorithm is an effective way to achieve pedestrian positioning in a Pedestrian dead reckoning algorithm is an effective way to achieve pedestrian positioning in a closed environment. In PDR algorithm, the step frequency and length of penetration are estimated closed environment. In PDR algorithm, the step frequency and length of penetration are estimated based on information of acceleration, respectively. The pedestrian’s movement direction can be based on information of acceleration, respectively. The pedestrian’s movement direction can be determineddetermined from from the the gyroscope’sgyroscope’s output. output. Thus Thus the theposition position of pedestrian of pedestrian can be calculated can be calculated based on based on the step, step step length length and and heading. heading. The The pedestrian pedestrian positioning positioning model model based based on PDR on is PDR expressed is expressed as follows:as follows: In FigureIn Figure1, black1, black marks marks represent represent footprint of of pedestrian, pedestrian, and and arrows arrows indicate indicate the direction the direction of of pedestrianpedestrian movement. movement. LLkk and φϕk krepresentrepresent pedestrian’s pedestrian’s step step length length and and heading heading at step at k step. (xk, ky.(k) xk, yk) representsrepresents the the pedestrian pedestrian position position at step step kk. .Pedestrian Pedestrian positioning positioning equation equation is shown is shown as follows: as follows:

xxLkkkk1 sin xk+1 = xk + Lk sin ϕk (1) yyL cos (1) yk+1kkkk=1 yk + Lk cos ϕk

Figure 1. Pedestrian dead reckoning model. Figure 1. Pedestrian dead reckoning model.

SensorsSensors2019 2019,, 1919,, 294x FOR PEER REVIEW 4 of4 of22 21

2.1.2. Pedestrian Positioning System Model 2.1.2. Pedestrian Positioning System Model Pedestrian indoor positioning model describes the whole process from data acquisition to Pedestrian indoor positioning model describes the whole process from data acquisition to position position output, as shown in Figure 2. In this model, MEMS-IMU part integrates several inertial output, as shown in Figure2. In this model, MEMS-IMU part integrates several inertial sensors such sensors such as accelerometer, magnetometer, and gyroscope. The raw data of acceleration, angular as accelerometer, magnetometer, and gyroscope. The raw data of acceleration, angular velocity and velocity and magnetic field strength acquired through the MEMS-IMU suffers seriously from low magnetic field strength acquired through the MEMS-IMU suffers seriously from low signal to noise signal to noise ratio. Therefore, it is necessary to preprocess the raw signal by wavelet denoising. ratio. Therefore, it is necessary to preprocess the raw signal by wavelet denoising. After preprocessing, After preprocessing, the information of acceleration is used to estimate steps and step length. theAccelerometer information of and acceleration gyroscope is used complement to estimate each steps other and step to get length. the Accelerometer initial attitude and angle gyroscope by complementcomplementary each filter, other which to get the then initial are attitude taken as angle measured by complementary value of robust filter, adaptive which then Kalman are taken filter as measured(RAKF). To value suppress of robust the adaptiveinterference Kalman of outliers, filter (RAKF). RAKF is To proposed suppress based the interference on adaptive of Kalman outliers, filter, RAKF isand proposed optimal basedheading on angle adaptive φ is obtained. Kalman filter, According and optimal to the information heading angle of stepϕ is n obtained., length l and According heading to theangle, information the pedestrian’s of step positionn, length isl calculatedand heading based angle, on theEquation pedestrian’s (1). position is calculated based on Equation (1).

Figure 2. Pedestrian positioning system schematic. MEMS-IMU: microelectromechanical-inertial measurementFigure 2. Pedestrian unit; RAKF: positioning robust adaptive system schematic.Kalman filter. MEMS-IMU: microelectromechanical-inertial measurement unit; RAKF: robust adaptive Kalman filter. 2.2. Signal Preprocessing 2.2. Signal Preprocessing The MEMS inertial system is widely used in positioning systems because of its low cost, light weight,The and MEMS complete inertial autonomy. system is However, widely used the in inertial positioning sensor systems is easily because affected of by its the low determination cost, light errorweight, and and the complete random error.autonomy. The determination However, the errorinertial is sensor closely is related easily toaffected the MEMS by the structure determinat andion the processingerror and the technology, random error. and reducingThe determination this error error is expensive is closely and related difficult. to the Therefore, MEMS structure random and error’s the processingprocessing oftechnology, inertial sensors and reducing becomes this the error focus is expensive of MEMS and data difficult. processing Therefore, [41]. Typical random denoising error’s methodsprocessing include of inertial digital sensors filtering becomes and wavelet the focus denoising. of MEMS Digital data filteringprocessing separates [41]. Typical the wanted denoising signal andmethods noise include to denoise digital without filtering overlapping and wavelet the denoising. spectrum Digital of the filtering signal and separates the spectrum the wanted of the signal noise. Theand spectrumnoise to denoise of the actual without situation overlapping signal andthe spectrum the spectrum of the of signal the noise and tend the tospectrum overlap. of For the example, noise. The spectrum of the actual situation signal and the spectrum of the noise tend to overlap. For example, the spectrum of Gaussian white noise is distributed almost in the entire frequency domain. Wavelet the spectrum of Gaussian white noise is distributed almost in the entire frequency domain. Wavelet denoising can be used to well separate random noise. Therefore, the original signal is preprocessed by denoising can be used to well separate random noise. Therefore, the original signal is preprocessed wavelet denoising. by wavelet denoising. As an effective signal digital processing method, wavelet denoising is based on the different As an effective signal digital processing method, wavelet denoising is based on the different characteristicscharacteristics of of signals signals and and noise noise at various at various scales. scales. The wavelet The wavelet coefficients coefficients are obtained are obtained by following by certainfollowing threshold certain threshold criteria to criteria achieve to effective achieve effective denoising. denoising. Wavelet Wavelet denoising denoising can be dividedcan be divided into the followinginto the following three steps: three steps: (1)(1) Wavelet decomposition of of signals signals containing containing noise: noise: select select the the appropriate appropriate wavelet wavelet base base and and decomposition layer; layer; (2)(2) Threshold quaquantizationntization processing: processing: select select appropriate appropriate thresholds thresholds and and threshold threshold functions functions to to process the coefficients coefficients of each each layer; layer; (3) Wavelet reconstruction: reconstruct the processed coefficients to obtain the denoised signal. (3) Wavelet reconstruction: reconstruct the processed coefficients to obtain the denoised signal. In the process of wavelet denoising, the choice of threshold is very important, usually using hard In the process of wavelet denoising, the choice of threshold is very important, usually using threshold and soft threshold method. The hard threshold method is prone to edge blurring and Gibbs hard threshold and soft threshold method. The hard threshold method is prone to edge blurring and phenomenon; the soft threshold method changes the degree of approximation between the Gibbsreconstructed phenomenon; signal and the softthe original threshold signal. method The improved changes the threshold degree function of approximation is proposed between from the the literature [42]:

Sensors 2019, 19, 294 5 of 21 reconstructed signal and the original signal. The improved threshold function is proposed from the literature [42]: m 1 λ2 u − 2 sgn(u) m−1 , |u| ≥ λ2 um ( ) = { 1 λ2(|u|−λ1) T u, λ1, λ2, m 2 sgn(u) m−1 , λ1 ≤ |u| ≤ λ2 (2) (λ2−λ1) 0, |u| ≤ λ1 where T is the denoising operation, that is, by the threshold and the threshold function, filtering out the noise coefficient and retaining the detail coefficient. u is the wavelet coefficient before denoising, and λ1 and λ2 are the lower threshold and the upper threshold. The adjustment parameter m can change the lower threshold so that the improved threshold function tends to be between the soft threshold function and the hard threshold function, having both the advantages of them.

2.3. Pedestrian Dead Reckoning Pedestrian dead reckoning uses the inertial measurement unit to calculate the position of the next moment based on current position. It has advantages of simple structure, small size, full autonomy, and strong anti-interference characteristics. The algorithm includes three important parts: step estimation, length estimation, and heading estimation.

2.3.1. Step Estimation Acceleration has periodicity during normal pedestrian motion, and peak detection is an effective means to determine the pedestrian’s step. In order to avoid the influence of system errors and the specific orientation of the accelerometer, the total acceleration A of the three axes is calculated: q 2 2 2 A = Ax + Ay + Az (3) where Ax, Ay, Az represent the corresponding acceleration of three axes of accelerometer. The key to peak detection is to avoid interference from abnormal peaks, and conventional peak detection uses a fixed time window to constrain the peaks, which ignores the constraints of adjacent peaks on acceleration. In this paper, the multi-threshold is used to constrain the acceleration peak to improve the accuracy of the step estimation. A set of constraints (p, v, ∆pt, ∆vt, |∆p|, |∆v|) represent peaks, valleys, time differences of adjacent peak, time differences of adjacent valley, differences of adjacent peak-to-peak, and differences of adjacent valley-to-valley. The effective peak and valley discriminant equations are shown as follows: ( 1, 10 < p < p& 4 p − < 4p < 4p +&0 < |4p| < 4p peak = η tη t tη η (4) 0, others wherein peak = 1 indicates it is an effective peak; and peak = 0 indicates a pseudo peak. (pη, ∆ptη−, ∆ptη+, ∆pη) is the threshold group for peak detection. ( 1, v < v < 10& 4 v − < 4v < 4v +&0 < |4v| < 4v valley = η tη t tη η (5) 0, others wherein valley = 1 indicates it is an effective valley; and valley = 0 indicates a pseudo valley. (vη, ∆vtη−, ∆vtη+, ∆vη) is the threshold group for valley detection. When stationary, the accelerometer is affected by the acceleration of gravity, and the total acceleration A is approximately equal to 10. The cycle of foot movement starts with the footstep off the ground and ends with the footstep touching the ground again. At the start, the upward acceleration is opposite to the direction of gravity acceleration, and the direction is the same when re-contacting, so set pη > 10 and vη < 10. An effective peak and a valley adjacent to each other constitute an effective step. Sensors 2019, 19, x FOR PEER REVIEW 6 of 22

In summary, the step detection based on the triple constraint condition considers the constraint of the adjacent peaks on the pedestrian acceleration, which can avoid the misjudgment problem of Sensorsthe traditional2019, 19, 294 PDR gait detection method, and thus realize the accurate judgment of the step number.6 of 21

2.3.2.In Length summary, Estimation the step detection based on the triple constraint condition considers the constraint of the adjacentA precise peaks accurate on the dynamic pedestrian length acceleration, estimation whichmodel canallows avoid researchers the misjudgment to improve problem the accuracy of the traditionalof pedestrian PDR dead gait reckoning. detection method,The authors and thusin [43] realize use the the nonlinear accurate judgment step size ofmodel the stepto estimate number. the step length, and the improved model is in this paper is effective for step length estimation of different 2.3.2.pedestrian Length individuals. Estimation A precise accurate dynamic length estimation model allows researchers to improve the accuracy of pedestrian dead reckoning. The authorsLAAAA 4 in [43] use the nonlinear() step size model to estimate the(6) t maxminmaxmin step length, and the improved model is in this paper is effective for step length estimation of different pedestrianAs known individuals. to all, different individuals have different parameters α and β. To enable this model p4 suitable for different individual,Lt = α αandA βmax should− Amin be +obtainedβ(Amax by− Aperformingmin) the least-multiplication(6) fitting of the training data before positioning. As known to all, different individuals have different parameters α and β. To enable this model suitable for different individual, α and β should be obtained by performing the least-multiplication 2.3.3. Heading Estimation fitting of the training data before positioning. The heading estimate is a crucial part of the pedestrian dead reckoning algorithm, which is 2.3.3.calculated Heading from Estimation measurement data of strapdown MEMS-IMU. A data fusion algorithm of attitude angleThe can heading be roughly estimate divided is a crucial into twopart of categories the pedestrian. One is dead based reckoning on the algorithm, frequency which domain is calculatedcharacteristics, from the measurement typical algorithm data of of strapdownwhich is complementary MEMS-IMU. Afilter. data This fusion fusion algorithm algorithm of does attitude not angleneed canto consider be roughly the dividedstatistical into characteristics two categories. of Onethe signal is based and on noise. the frequency The other domain is to design characteristics, filters in thethe typicaltime domain algorithm using of whichthe state is- complementaryspace approach, filter. such This as the fusion Kalman algorithm filter. As does complementary not need to consider filters thegenerally statistical do not characteristics require an accurate of the signal noise and model, noise. it Thehas otheradvantages is to design of less filters computation in the time and domain higher usingaccuracy. the state-spaceHowever, complementary approach, such asfilters the Kalmancannot suppress filter. As the complementary measurement filters of outliers. generally Therefore, do not requirethis paper an accurateproposes noise a fusion model, filter it hasalgorithm advantages consisting of less of computation complementary and higherfilter and accuracy. robust However, adaptive complementaryKalman filter (RAKF). filters cannotTake pitch suppress θ as an the example, measurement and the of outliers.fusion filter Therefore, model is this shown paper as proposes follows: a fusion(1) Complementary filter algorithm Filter consisting of complementary filter and robust adaptive Kalman filter (RAKF). Take pitch θ as an example, and the fusion filter model is shown as follows: MEMS inertial sensors have some inherent problems. There is a static drift in the gyroscope, and (1)errors Complementary will accumulate Filter when the attitude is calculated; the dynamic response of the accelerometer and magnetometerMEMS inertial is sensors poor, but have there some is no inherent cumulative problems. error. ThereTherefore, is a staticthe attitude drift in of the accelerometer gyroscope, andand errorsmagnetometer, will accumulate the attitude when theof gyrosc attitudeope is calculated;are calculated the dynamicunder static response and dynamic of the accelerometer conditions, andrespectively, magnetometer and then is poor, the outputs but there are is complementary no cumulative error. to each Therefore, other to theimprove attitude the of accuracy accelerometer of the andattitude. magnetometer, Complementary the attitude filter algorithm of gyroscope is shown are calculatedin Figure 3 under in details. static and dynamic conditions, respectively, and then the outputs are complementary  to each other to improve the accuracy of the attitude. Complementary filter algorithm is shownerr in Figure a 3 in details. t ' aKK p  err i  err θerr = θa − θ 0 (7) θ0 = K θ  + R t K θ dt (7) a p err 0 i err  ' θ = τ θ + gadt θ0 τ+dtgdtτ+dt a dt

FigureFigure 3.3. FusionFusion ﬁlterfilter model.model. Because the low-passband attenuation of the complementary ﬁlter is slow, and the error is large when the noise is large, the Proportional Integral(PI) control is added on the basis of the complementary Sensors 2019, 19, 294 7 of 21

filter. The error between the attitude angle calculated by accelerometer and magnetometer, and the post-filtering attitude angle are PI-controlled. Then the complementary filter algorithm is used to fuse the attitude information and improve the accuracy of the attitude. In Figure3, τ/τ + dt is high-pass filter and dt/τ + dt is low-pass filter. The output of the accelerometer and magnetometer passes through a low-pass filter to limit the high-frequency jitter in the attitude measurement; accumulated drift error of the gyroscope can be suppressed through the high-pass filter. The parameters are selected according to reference [44]. Kp is the switching frequency of the low-pass filter and the high-pass filter, which is selected according to the frequency characteristics of the accelerometer and the output of the 2 gyroscope, and Kp = Ki /2. The parameters Kp, Ki of the complementary filter in this paper are set to 0.7 and 1.2 respectively, where the error correction effect is obvious. (2) Robust Adaptive Kalman Filter Complementary filter complements accelerometer, magnetometer with gyroscope to suppress cumulative errors, which improves the accuracy of heading. However, the complementary algorithm is limited by the inaccurate measurement of the sensors. The outliers in the measured value cannot be identified and the interference cannot be suppressed, which severely reduces the accuracy and affects stability of the positioning system. On the basis of a complementary filter, the robust adaptive Kalman filter algorithm is proposed in this paper. The post-complementary filtered attitude and Optimal attitude are measured value and state value, respectively, to suppress the influence of the measured outliers on the heading estimation. The basic idea of Kalman filter is that based on the state model of signal and noise, the estimate of the current state variable is updated using the previous time’s estimated value and the current time’s observation value. The equation of state for the pedestrian positioning system is described below:    1 sin φ0k cos φ0k tan θ0k wxk    x(k + 1) = x(k) +  0 cos φ0k − sin φ0k  wyk  + w(k) (8) 0 sin φ0k sec θ0k cos φ0k sec θ0k wzk where x(k) = [Φ0(k), θ0(k), ψ0(k)] is the state variable, w(k) represents system noise vector, which is ignored in this stable positioning system. The complementary filtered attitude angles are selected as the measured value Z(k) = [Φ(k), θ(k), ψ(k)], the observation equation is shown as follow:

Z(k + 1) = H(k + 1)x(k) + v(k + 1) (9) where H(k) = diag [1, 1, 1], and v(k + 1) is the observation noise vector. The Kalman ﬁlter consists of a prediction process and an update process. The calculation steps are as follows: (1) Forecasting process: State forecast Xˆ k,k−1 = Φk,k−1Xˆ k−1 (10) Covariance matrix of state prediction

T Pk,k−1 = Φk,k−1Pk−1Φk,k−1 + Qk−1 (11)

(2) Updating process: Gain matrix −1 Th T i Kk = Pk,k−1 Hk HkPk,k−1 Hk + Rk (12) Sensors 2019, 19, 294 8 of 21

State estimation Xˆ k = Xˆ k,k−1 + Kk Zk − HkXˆ k,k−1 (13) Covariance matrix of state estimation

T T Pk = [I − Kk Hk]Pk,k−1[I − Kk Hk] + KkRk−1Kk (14)

By adding a measurement noise estimator to the traditional Kalman [45], the statistical properties of the measurement noise can be estimated and corrected in real time.

n T T To Rk = (1 − dk)Rk−1 + dk [I − HkKk−1]ekek [I − HkKk−1] + HkPk−1 Hk (15)

1 − b d = (16) k 1 − bk+1 where in b is forgetting factor, and the range of b is 0.95–0.99 in reference [45], which is set to 0.98 in the paper; the deﬁnition innovation matrix ek and the corresponding covariance matrix Ck at time k are:

ek = Zk − HkXˆ k/k−1 (17)

T Ck = HkPk,k−1 Hk + Rk (18) When the system is not faulty, the innovation is a zero-mean white noise sequence. If the actual statistical characteristics of the innovation are not consistent with the theory, the process is considered to be abnormal. In terms of fault detection, the innovation χ2 detection algorithm is an effective method. Assume test statistic T = eTC−1e k k k k (19) 2 The innovation ek is a normal distribution with zero mean, so Tk obeys the χ distribution with a degree of freedom m (m is the dimension of the observation). Transform the fault detection condition into a hypothesis test condition, according to the deﬁnition of the χ2 distribution:

( 2 H0 : Tk ∼ χ (m, 0) 2 (20) H1 : Tk ∼ χ (m, λ)

In the formula, H0 indicates that there is no gross error in the observed value, H1 indicates that the observed value has an abnormal value, and λ is a non-centralized parameter. The significance level α indicates the probability that the null hypothesis is established but rejects the hypothesis selection alternative hypothesis. The selection of the significance level determines the threshold of the innovation statistic. The smaller the value of α, the larger the threshold, and too large or too small threshold will affect the system’s robust performance. A suitable value for the alpha should be in the 2 range of 0.01 to 0.15, and we make α = 0.1 in the paper. The boundary condition is set to TD = χ a (m, 0). If Tk ≤ TD, it is judged that there is no abnormality in the observation. Otherwise, there is an abnormal value in the observation. It is necessary to introduce a weighting factor to amplify the new covariance and reduce the influence of the wild value on the filtering. Construct weighting factor using Huber functions: ( 1 : Tk,i ≤ TD αi = TD (αi ≤ 1) (21) : T TD Tk,i k,i

And a = diag [a1, a2 ... an], I = 1, 2 ... n. The equivalent covariance of the observations is updated to R R = k (22) k α Sensors 2019, 19, 294 9 of 21

It can be seen from Equation (12) that the adaptive innovation weighting factor (ai ≤ 1) reduces the gain matrix when the observed gross error occurs. The weight of the gross error in the state estimation is reduced, the interference of the wild value on the ﬁltering is suppressed, and the fault tolerance of Sensors 2019, 19, x FOR PEER REVIEW 10 of 24 the system is improved. The whole RAKF algorithm is shown as below in details in Figure4:

Figure 4. RAKF algorithm.

3. Results

3.1. Laboratory Equipment The Mti series MEMS-IMU from Xsens is used in the experiment, which is ﬁxed on the instep. The MEMS-IMU is mainly composed of a triaxle orthogonal accelerometer, a triaxle orthogonal magnetometer and a triaxle orthogonal gyroscope. Keep the foot unmoved for 1 s before the start of the experiment to coincide with the starting point. The MEMS-IMU used in the experiment meets the experimental requirements.

Sensors 2019, 19, x FOR PEER REVIEW 10 of 22

3. Results

3.1. Laboratory Equipment The Mti series MEMS-IMU from Xsens is used in the experiment, which is fixed on the instep. The MEMS-IMU is mainly composed of a triaxle orthogonal accelerometer, a triaxle orthogonal magnetometer and a triaxle orthogonal gyroscope. Keep the foot unmoved for 1 s before the start of the experiment to coincide with the starting point. The MEMS-IMU used in the experiment meets the experimental requirements.

3.2. Analysis of Signal Preprocessing In order to verify the validity of the wavelet decomposition, the angular velocity of the gyro X-axis in the static case is selected for analysis. In this paper, the wavelet decomposition selects ‘db6′ as the wavelet basis function according to reference [42]. The number of decomposition layers of 3 to 9 Sensorswavelets2019 , is19 , selected 294 for comparative study, and the optimal decomposition layer number 10 of of 9 21 is selected. The wavelet coefficients are processed by the improved threshold function. The parameters of improved threshold function are set according to the literature [42], where upper threshold λ2 is a improved threshold function are set according to the literature [42], where upper threshold λ2 is a ﬁxed fixed value 0.8; m is an integer from 2–13; and lower threshold λ1 is set to λ2/(m + 1). Draw a value 0.8; m is an integer from 2–13; and lower threshold λ is set to λ /(m + 1). Draw a comparison comparison of the original signal and the reconstructed1 colored 2 noise, as shown in Figure 5. of the original signal and the reconstructed colored noise, as shown in Figure5. Compared with the Compared with the original signal, the reconstructed signals are all closer to the true value, no matter original signal, the reconstructed signals are all closer to the true value, no matter what threshold what threshold functions are used, which proves the filtering effectiveness of the wavelet functions are used, which proves the ﬁltering effectiveness of the wavelet decomposition. Compared decomposition. Compared with traditional threshold functions, the improved threshold function in [42] with traditional threshold functions, the improved threshold function in [42] perform better, which is perform better, which is adopted in this paper to improve the credibility of data. adopted in this paper to improve the credibility of data.

Figure 5. Comparison of original signal and reconstructed signals based on different thresholds. Figure 5. Comparison of original signal and reconstructed signals based on different thresholds. 3.3. Analysis of Improved PDR Algorithm 3.3. Analysis of Improved PDR Algorithm In order to verify the effectivity of the proposed improved PDR algorithm, step estimation, step lengthIn order estimation, to verify the and effectivity heading of estimation the proposed are tested improved separately PDR algorithm, before the step formal estimation, experiment. step Thelength experimental estimation, siteand isheading arranged estimation on the secondare tested floor separately of the entire before School the formal of Internet experiment. of Things The Engineering.experimental The site satelliteis arranged image on of the the second experimental floor of site the is entire shown School in Figure of 6 Interneta. The structure of Things andEngineering. installation The of satellite the pedestrian image of inertial the experimental navigation site system is shown is shown in Figure in Figure 6a. T6b.he Astructure three-axis and accelerometer,installation of three-axis the pedestrian gyroscope, inertial and navigation three-axis magnetometer system is shown are mounted in Figure on 6b. the AI2C three bus- andaxis data are transmitted from the serial port to the host computer through the DSP. Acceleration, angular rate, and magnetic field strength measurements are obtained. The inertial navigation module is tied on the foot to obtain pedestrian movement information. In Figure6a, the red circle represents the starting point and the ending point. The direction of movement is indicated by the arrow. The area enclosed by the black dotted line is used to test the performance of the step estimation algorithm. The MEMS-IMU is fixed on the back of the pedestrian, and the subject moves along the arrow as shown in Figure6b. Sensors 2019, 19, x FOR PEER REVIEW 11 of 22 accelerometer, three-axis gyroscope, and three-axis magnetometer are mounted on the I2C bus and data are transmitted from the serial port to the host computer through the DSP. Acceleration, angular Sensorsrate, and2019 magnetic, 19, 294 field strength measurements are obtained. The inertial navigation module is11 tied of 21 on the foot to obtain pedestrian movement information.

(a)

(b)

Figure 6. (a) Satellite image of the experimental site (b). Inertial measurement unit (IMU) structure Figureand installation 6. (a). Satellite diagram. image of the experimental site (b). Inertial measurement unit (IMU) structure and installation diagram. 3.3.1. Step Frequency Analysis InThe Figure multi-threshold 6a, the red methodcircle represents is usedto the ameliorate starting point the step and estimationthe ending algorithm point. The in direction this paper, of movement is indicated by the arrow. The area enclosed by the black dotted line is used to test the which can avoid interference from abnormal peaks. The threshold group (pη, ∆ptη−, ∆ptη+, ∆pη) for performance of the step estimation algorithm. The MEMS-IMU is fixed on the back of the pedestrian, peak detection is set to (30, 0.6, 1.8, 20); and the threshold group for valley detection (vη, ∆vtη−, ∆vtη+, and the subject moves along the arrow as shown in Figure 6b. ∆vη) is set to (8, 0.6, 1.8, 5). In order to verify the validity of the algorithm, the pedestrian moves at the above experimental 3.3.1.site, and Step the Frequency acceleration Analysis information of the strap-down MEMS-IMU is obtained. Then, the wavelet denoisingThe multi is adopted-threshold to improve method reliability is used to of ameliorate acceleration the data. step Finally,estimation the newalgorit datahm is in processed this paper, to whichobtain can the totalavoid acceleration interference of from the threeabnormal axes, peaks. and the The multi-threshold threshold group method (pη, Δp istη used−, Δptη+ to, Δ discriminatepη) for peak detectionthe effective is set peaks to (30, and 0.6, valleys, 1.8, 20); which and form the threshold effective steps,group as for shown valley in detection Figure7. (vη, Δvtη−, Δvtη+, Δvη) is setFigure to (8, 0.6,7a is 1.8, the 5) global. processed acceleration waveform, and Figure7b is a partial enlarged view of theIn ﬁrst order 15 to s part.verify In the Figure validity7, it of is the obvious algorithm, that A the is approximatelypedestrian moves equal at the to 10above when experimental stationary. Accordingsite, and the to acceleration the previously information proposed stepof the estimation strap-down algorithm, MEMS- effectiveIMU is obtained. peaks and Then, valleys the are wavelet found denoisingas shown inis adopted Figure7b, to which improve are reliability marked with of acceleration red squares, data. and Finally, adjacent the effective new data peak is processed and valley to can form an effective step. After multiple tests, the average detection accuracy is roughly 99%, which prove the stability and effectiveness of the step estimation algorithm.

Sensors 2019, 19, x FOR PEER REVIEW 12 of 22

Sensors 2019, 19, 294 12 of 21 obtain the total acceleration of the three axes, and the multi-threshold method is used to discriminate the effective peaks and valleys, which form effective steps, as shown in Figure 7.

(a) (b)

FigureFigure 7. 7.StepStep estimation. estimation. (a ()a Simulation) Simulation diagram diagram of of step step detection; detection; ( (b)) Partial magniﬁcationmagnification simulation. simulation.

3.3.2.Figure Step Length7a is the Analysis global processed acceleration waveform, and Figure 7b is a partial enlarged view of theThe first parameters 15 s part. Inα andFigureβ in 7 Equation, it is obvious (6) vary that from A is personapproximately to person equal due to to factors 10 when such stationary as height,. Accordingweight, and to gender, the previously so different proposed individuals step estimation must be trained algorithm, to obtain effective the peaks applicable and valleysparameters are foundbefore as positioning. shown in Figure In this 7b, paper, which two are subjects marked participated with red squares, in length and experiment, adjacent effective subject 1 peak is female, and valley165 cm can height, form an 56 effective kg weight; step. subject After multiple 2 is male, tests, 176 the cm, average and 72 detection kg. Two accuracy subjects is are roughly required 99%, to whichmove prove at random the stability to record and training effectiveness data of as the much step as estimation possible. algorithm. Then the training data including step length and acceleration information is processed by the least squares method to obtain the 3.3.2.corresponding Step Length parameters Analysis α and β. The parameters α and β of subjects are −0.6291 and 0.0868, −0.7291,The parameters and 0.0922, α respectively. and β in Equation (6) vary from person to person due to factors such as height, weight,For and quantitative gender, so analysis, different two individuals subjects performed must be trained multiple to obtain tests in the site applicable shown in parameters Figure6b, beforeincluding positioning. status of In walking this paper, and two running. subjects Subjects participated move fromin length red frame, experiment, along directionsubject 1 is of female, arrows 165in a cm ﬁxed height, step 56 of 1kg m w (lengtheight; subject of 2 tiles), 2 is andmale, the 176 realistic cm, and distance 72 kg. Two (RD) subjects for length are estimation required to is move about at181m. random Data to isrecord processed training using data the as length much estimationas possible. modelThen the in referencetraining data [43] including and improved step length model andof this acceleration paper, respectively. information The is stepprocessed length by and the error least of squares two subjects method are to intuitively obtain the compared corresponding in the parametersFigure8. Finally, α and theβ. The solution parameters distance α and (SD) β andof subjects mean absolute are −0.6291 error and (MAE) 0.0868, of −0.7291, length ofand both 0.0922, two respectively.subjects processed by different models are calculated. The results of same subject in the same state are averagedFor quantitative and shown inanalysis, Table1 , two where subjects Run-1 performedrepresents the multiple results tests of subject in site 1 under shown running. in Figure 6b, includingIn Figure status8, of the walking left and and right running. part representsSubjects move the from measured red frame, results along in walking direction and of arrows running in arespectively. fixed step of Through 1 m (length observing of 2 tiles), each singleand the step, realistic the length distance estimation (RD) for accuracy length of estimation improved is model about is 181m.generally Data a is little processed better than using the the original length model,estimation although model not in veryreference obvious. [43] Inand qualitative improved analysis model of of thisboth paper, two models, respectively. experimental The step data length of the and entire error path, ofwhich two subjects contains are both intuitively subjects iscompared used to calculate in the Figuremean absolute8. Finally, error the andsolution Solution distance distance. (SD) As and shown mean in absolute Table1, the error improved (MAE) modelof length performs of both better two subjectsthan the processed original model, by different regardless models of subjects are calculated. or motions. The The results accuracy of same of lengthsubject estimation in the same of run-2state areis relatively averaged poor, and shown but the in accuracy Table 1, is where still higher Run-1 than represents 94%. The the improved results of modelsubject can 1 under provide running. accurate length estimation in PDR algorithm, where parameters α and β need to be determined by individual Tablein advance. 1. Step length analysis of two subjects using different models. RD: realistic distance; SD: solution distance; MAE: mean absolute error. Table 1. Step length analysis of two subjects using different models. RD: realistic distance; SD: solution Original model Improved model distance; MAE: mean absoluteRD error. [m] MAE [m] SD [m] MAE [m] Run-1 181.0 195.1Original Model0.13 189.3 Improved 0.08 Model Walk-1 181.0 RD174.3 [m] MAE0.09 [m]175.6 SD [m]0.07 MAE [m] Run-1Run-2 181.0181.0 200.9 195.1 0.15 0.13 191.5 189.3 0.11 0.08 Walk-1Walk-2 181.0181.0 207.5 174.3 0.18 0.09 187.1 175.6 0.12 0.07 Run-2 181.0 200.9 0.15 191.5 0.11 Walk-2 181.0 207.5 0.18 187.1 0.12

SensorsSensors2019 2019, ,19 19,, 294 x FOR PEER REVIEW 1313 of of 22 21

FigureFigure 8. 8. StepStep lengthlength and error comparison of two subjects subjects in in different different motions motions using using different different models. models.

DifferentIn Figure individuals 8, the left need and right corresponding part represents parameters. the measured In order to results highlight in walking the importance and running of the correspondingrespectively. Through parameters, observing the parameters each single of subjectstep, the 1 areleng usedth estimation to solve subject accuracy 2, andof improved the results model under twois generally different a motions little better are boththan shownthe original as follows: model, the although SD and not MAE very of subjectobvious. 2 underIn qualitative running analysis are 238.2 andof both 0.32, two respectively. models, Inexperimental the walking data state, of the the solution entire path, distance which and contains mean absolute both subjects error are is 231.8used andto 0.27,calculate respectively. mean absolute Compare error the solution and Solution results distance. of subject As 2 using shown two in different Table 1, parameters, the improved it is obvious model thatperforms the results better using than the the corresponding original model, parameters, regardless no of matter subjects original or motions. or improved The accuracy model isof used, length are muchestimation better of than run- the2 isresults relatively under poor, mismatched but the accuracy parameters. is still higher Therefore, than it94%. is necessary The improved that different model individualscan provide should accurate obtain length the estimation matching parameters in PDR algorithm, before performing where parameters the positioning α and experiments.β need to be determined by individual in advance. 3.3.3. Heading Analysis Different individuals need corresponding parameters. In order to highlight the importance of the correspondingThe improved parameters, heading estimation the parameters is the of core subject of the 1 are proposed used to PDRsolve algorithm,subject 2, and inheriting the results the advantagesunder two different of complementary motions are filtering both shown (CF) and as follows: robust adaptivethe SD and Kalman MAE filter.of subject In the 2 under PDR algorithm, running subtleare 238.2 heading and 0.32, deviation respectively. will cause In the pedestrian walking tostate, deviate the solution significantly distance from and the mean real position absolute because error ofare the 231.8 cumulative and 0.27, effect, respectively. which severely Compare affects the sol theution accuracy results of of the subject inertial 2 using positioning two different system. Theparameters, heading it tests is obvious are carried that the out results in the using above the environment, corresponding and parameters, Figure9 shows no matter comparisons original or of performanceimproved model of different is used, algorithms are much better in terms than of the heading results error. under mismatched parameters. Therefore, it is necessary that different individuals should obtain the matching parameters before performing the positioning experiments.

Sensors 2019, 19, x FOR PEER REVIEW 14 of 22

3.3.3. Heading Analysis The improved heading estimation is the core of the proposed PDR algorithm, inheriting the advantages of complementary filtering (CF) and robust adaptive Kalman filter. In the PDR algorithm, subtle heading deviation will cause pedestrian to deviate significantly from the real position because of the cumulative effect, which severely affects the accuracy of the inertial positioning system. The heading tests are carried out in the above environment, and Figure 9 shows comparisons of Sensors 2019, 19, 294 14 of 21 performance of different algorithms in terms of heading error.

(a) (b)

Figure 9. Heading analysis in different ﬁlteringfiltering strategies. ( a)) Comparison of heading; ( b) Heading error comparison.

As shown in Figure 99,, thethe performanceperformance ofof complementarycomplementary filteringfiltering isis notnot goodgood enough,enough, whosewhose ◦ MAE is 18.6 °.. Compared Compared to to complementary complementary filtering, filtering, CF CF + + KF KF performs performs better, better, but but still still suffers suffers from interferenceinterference of abnormal values. In In contrast, contrast, fusion fusion filter filter introduces introduces RAKF with complementary filtering,filtering, which which can can effectively effectively reduce reduce the the influence influence of of outliers outliers on on filtering filtering to toimprove improve the the accuracy accuracy of PDRof PDR positioning positioning system. system. As Asshown shown in Figure in Figure 9b,9 theb, the dynamic dynamic error error of complementary of complementary filter filter and and CF ◦ +CF KF + are KF are−58.6−–58.6–12.812.8° and and−10.8−–10.8–39.8,39.8, showing showing that there that there are individual are individual outliers. outliers. In comparison, In comparison, the headingthe heading dynamic dynamic error error of ofthe the fusion fusion filter filter is is−18.1−18.1–9.8,–9.8, indicating indicating that that individual individual outliers outliers are effectively suppressed.

3.3.4. Trajectory Trajectory Comparison Analysis In order order to to verify verify the the performance performance of the of complete the complete PDR positioning PDR positioning system combined system combined of improved of improvedstep estimation, step estimation, adaptive step adaptive length step model, length and model, fusion and filtering, fusion several filtering, sets several of pedestrian sets of positioningpedestrian positioningexperiments experiments are carried outare indoors.carried out indoors. Multiple tests ofof pedestrianpedestrian positioning positioning are are performed performed under under status status of of walking walking and and running running inthe in thepresented presented location, location, respectively. respectively. The The measured measured data data is stored is stored and and then then processed processed evenly evenly to obtain to obtain the thefollowing following simulation simulation map, map, where where the the starting starting point point is preset is preset to (0, to 0)(0, in 0) matlab. in matlab. Figures Figure 10s and 10 and 11 are 11 areactual actual trajectories trajectories and error and comparisons error comparisons in the walking in the walkingstate; Actual state; trajectories Actual trajectories and error comparison and error comparisondiagrams in diagrams the running in state the running are shown state in are Figures shown 12 and in Figure 13. Fors 12 the and quantitative 13. For the analysis quantitative of the analysisimproved of PDR the positioningimproved PDR system, positioning the error range,system, mean the error absolute range, error, mean and rootabsolute mean error, square and error root of meanthe east square and north error directions of the east of and each north trajectory directions are calculated, of each trajectoryas shown in are Tables calculated,2 and3 . as shown in TableAccordings 2 and 3. to the trajectory map under walking, the pedestrian’s trajectory is slowly offset from the real trajectory due to the existence of the cumulative error, and the end point does not coincide with the preset end point. With fusion filter composed of CF and RAKF, the east and north error ranges are −9.8–6.3 m and −5.2–5.1 m, respectively. The mean absolute errors of east and north are 5.3 and 3.8 m. The error analysis with complementary filter and CF + KF are also shown in Table2. In contrast, the positioning accuracy of fusion filter composed of CF and RAKF is obviously better. The error of the improved PDR positioning system is lower than 2.5% of the total length under walking, which can be well used to conduct accurate positioning in the closed environment. According to the above figures and Table3, the positioning accuracy of fusion filter is significantly higher than single complementary filter and CF + KF, which is consistent with the results in the walking state. Sensors 2019, 19, x FOR PEER REVIEW 15 of 22 Sensors 2019, 19, 294 15 of 21 Sensors 2019, 19, x FOR PEER REVIEW 15 of 22

Figure 10. Comparison of trajectory in walking under different heading estimation algorithms Figure 10. Comparison of trajectory in walking under different heading estimation algorithms performedFigure in 10. second Comparison floor of ofentire trajectory Internet in of walking Things under(IOT) Engineering different heading College. estimation algorithms performedperformed in second in second ﬂoor of floor entire of entire Internet Internet of Thingsof Things (IOT) (IOT) EngineeringEngineering College. College.

Sensors 2019, 19, x FOR PEER REVIEW 16 of 22

Table 2. Error comparison table in walking under different heading estimation algorithm. CF: complementary filtering; KF: Kalman filter.

CF CF + KF CF + RAKF (a) (a) North East North East (b(b) ) North East Error Range [m] −4.8–15.1 −27.5–12.3 −2.7–14.6 −16.3–6.7 −5.2–5.1 −9.8–6.3 FigureFigure 11.Figure Trajectory11. Trajectory 11. Trajectory analysis analysis analysis in in walking walking in walking underunder under different different heading headingheading estimation estimation estimation algorithms. algorithms. algorithms. (a) North (a) North error comparison;MAE [m] (b) East error10.7 comparison.14.7 8.1 9.4 3.8 5.3 errorerror comparison; comparison; (b) ( Eastb) East error error comparison. comparison. According to the trajectory map under walking, the pedestrian’s trajectory is slowly offset from According to the trajectory map under walking, the pedestrian’s trajectory is slowly offset from the real trajectory due to the existence of the cumulative error, and the end point does not coincide the real trajectory due to the existence of the cumulative error, and the end point does not coincide with the preset end point. With fusion filter composed of CF and RAKF, the east and north error with the preset end point. With fusion filter composed of CF and RAKF, the east and north error ranges are −9.8–6.3 m and −5.2–5.1 m, respectively. The mean absolute errors of east and north are 5.3 ranges are −9.8–6.3 m and −5.2–5.1 m, respectively. The mean absolute errors of east and north are 5.3 and 3.8 m. The error analysis with complementary filter and CF + KF are also shown in Table 2. In and 3.8contrast, m. The the error positioning analysis accuracy with complementary of fusion filter composed filter and ofCF CF + andKF areRAKF also is shownobviously in better.Table 2.The In contrast,error the of thepositioning improved accuracy PDR positioning of fusion system filter composed is lower than of CF2.5% and of theRAKF total is length obviously under better. walking, The errorwhich of the can improved be well PDRused topositioning conduct accurate system positioning is lower than in the 2.5% closed of the environment. total length under walking, which can be well used to conduct accurate positioning in the closed environment.

Figure 12. Comparison of trajectory in running under different heading estimation algorithms Figure 12. Comparison of trajectory in running under different heading estimation algorithms performed in second ﬂoor of IOT Engineering College. performed in second floor of IOT Engineering College.

(a) (b)

Figure 13. Trajectory analysis in running under different heading estimation algorithms. (a) North error comparison; (b) East error comparison.

According to the above figures and Table 3, the positioning accuracy of fusion filter is significantly higher than single complementary filter and CF + KF, which is consistent with the results in the walking state.

Sensors 2019, 19, x FOR PEER REVIEW 16 of 22

Table 2. Error comparison table in walking under different heading estimation algorithm. CF: complementary filtering; KF: Kalman filter.

CF CF + KF CF + RAKF North East North East North East Error Range [m] −4.8–15.1 −27.5–12.3 −2.7–14.6 −16.3–6.7 −5.2–5.1 −9.8–6.3 MAE [m] 10.7 14.7 8.1 9.4 3.8 5.3

Figure 12. Comparison of trajectory in running under different heading estimation algorithms Sensors 2019, 19, 294 16 of 21 performed in second floor of IOT Engineering College.

(a) (b)

Figure 13. Trajectory analysis in running under different heading estimation algorithms. ( (aa)) North errorerror comparison; ( b) East error comparison.

According to the above figures and Table 3, the positioning accuracy of fusion filter is Table 2. Error comparison table in walking under different heading estimation algorithm. CF: significantly higher than single complementary filter and CF + KF, which is consistent with the results complementary ﬁltering; KF: Kalman ﬁlter. in the walking state. CF CF + KF CF + RAKF

North East North East North East

Error Range [m] −4.8–15.1 −27.5–12.3 −2.7–14.6 −16.3–6.7 −5.2–5.1 −9.8–6.3 Sensors 2019MAE, 19, x [m] FOR PEER REVIEW 10.7 14.7 8.1 9.4 3.8 5.3 17 of 22

TableTable 3. 3. ErrorError comparison comparison table table in in running running under under different different heading heading estimation estimation algorithm.

CFCF CFCF + + KF KF CF CF + +RAKF RAKF NorthNorth EastEast NorthNorth East North North East East Error Range[m] −4.7–21.3 −25.1–12.6 −8.5–10.3 −25.3–11.2 −4.6–8.0 −12.9–7.4 Error Range [m] −4.7–21.3 −25.1–12.6 −8.5–10.3 −25.3–11.2 −4.6–8.0 −12.9–7.4 MAEMAE [m] [m] 11.311.3 16.916.9 8.38.3 13.2 4.7 4.7 7.27.2

3.4. Additional Experiments 3.4. Additional Experiments In this section, in order to verify the scope of the proposed algorithm, the additional experiments In this section, in order to verify the scope of the proposed algorithm, the additional experiments in typical sites are performed using two kinds of sensors for testing and comparison. As shown in in typical sites are performed using two kinds of sensors for testing and comparison. As shown in Figure 14, one is the high-performance sensor Mti series MEMS-IMU from Xsens, and the other is the Figure 14, one is the high-performance sensor Mti series MEMS-IMU from Xsens, and the other is the low-cost IMU AH-100B. The typical sites are divided into complex and broad place. low-cost IMU AH-100B. The typical sites are divided into complex and broad place.

Figure 14. Experiment apparatus, Mti series MEMS-IMU from Xsens and AH-100B. Figure 14. Experiment apparatus, Mti series MEMS-IMU from Xsens and AH-100B.

Typical case 1: The complex site is selected in the laboratory, and the schematic diagram of the laboratory is shown in Figure 15a. Figure 15b displays the setting of experiment: The shadows represent the desks, the blanks represent the walkways, where subject moves. The red circle represents starting point, which is established as the point [0, 0], and the subject moves in the laboratory along the direction of the arrow. The experimental results of Mti series MEMS-IMU are shown in Figure 15c, and Figure 15d represents the positioning trajectory of AH-100B. According to the two figures, it is obvious that the proposed algorithm has better performance than the original model. At the same time, according to Figure 15d, the positioning accuracy of the improved algorithm is significantly improved compared with the traditional algorithm, but it is limited to the performance of the sensor to some extent.

Sensors 2019, 19, 294 17 of 21

Typical case 1: The complex site is selected in the laboratory, and the schematic diagram of the laboratory is shown in Figure 15a. Figure 15b displays the setting of experiment: The shadows represent the desks, the blanks represent the walkways, where subject moves. The red circle represents starting point, which is established as the point [0, 0], and the subject moves in the laboratory along the direction of the arrow. The experimental results of Mti series MEMS-IMU are shown in Figure 15c, and Figure 15d represents the positioning trajectory of AH-100B. According to the two ﬁgures, it is obvious that the proposed algorithm has better performance than the original model. At the same time, according to Figure 15d, the positioning accuracy of the improved algorithm is signiﬁcantly improved compared with the traditional algorithm, but it is limited to the performance of the sensor to someSensors extent. 2019, 19, x FOR PEER REVIEW 18 of 22

(a)

(b)

FigureFigure 15. Typical15. Typical experiment experiment 1. 1. ( a()a) Laboratory Laboratory under under panoramic panoramic view; view; (b) (experimentalb) experimental diagram. diagram. (c) experimental(c) experimental results results of Mtiof Mti series series MEMS-IMU MEMS-IMU fromfrom Xsens. Xsens. (d (d) Experimental) Experimental results results of AH of- AH-100B.100B.

Typical case 2: The wide experimental site is selected in the school library, and the schematic diagram of the library is shown below. The experimental results of Mti series MEMS-IMU are shown in Figure 16b, and Figure 16c represents the positioning trajectory of AH-100B.

Sensors 2019, 19, 294 18 of 21

Typical case 2: The wide experimental site is selected in the school library, and the schematic diagram of the library is shown below. The experimental results of Mti series MEMS-IMU are shown inSensors Figure 2019 16, 19b,, x and FORFigure PEER REVIEW 16c represents the positioning trajectory of AH-100B. 19 of 22

(a)

(b) (c)

FigureFigure 16. 16. TypicalTypical experiment experiment 2. (a 2.) Experimental (a) Experimental site; (b) site;Experimental (b) Experimental results of Mti results series of MEMS Mti series-IMU MEMS-IMUfrom Xsens. ( fromc) Experimental Xsens. (c) Experimental results of AH results-100B. of AH-100B.

AccordingAccording toto FigureFigure 1616,, thethe experimentalexperimental resultsresults performedperformed inin librarylibrary areare consistentconsistent withwith thethe resultsresults inin laboratory.laboratory. InIn somesome placesplaces wherewhere thethe requirementsrequirements areare notnot demanding,demanding, low-costlow-cost MEMSMEMS areare recommendedrecommended to to be be used, used, whose whose performance performance is reluctantly is reluctantly acceptable. acceptable. However, However, in some demanding in some places,demanding such places, as mines such and as ﬁre mines rescue, and itfire is recommendedrescue, it is recommended to use high-performance to use high-performance MEMS, which MEMS, can providewhich can accurate provide positioning accurate positioning using improved using model improved proposed model in proposed this paper. in this paper.

4.4. Conclusions InIn thisthis paper,paper, anan improvedimproved PDRPDR positioningpositioning systemsystem based on pure inertial sensors is pr proposed.oposed. FirstFirst ofof all, thanks to the development of MEMS-IMU,MEMS-IMU, portableportable sensorssensors cancan bebe carried around.around. In view ofof thethe low low signal-to-noise signal-to-noise ratio ratio of inertialof inertial sensors, sensors, wavelet wavelet decomposition decomposition is used is toused pre-ﬁlter to pre the-filter inertial the signalsinertial to signals improve to improve the credibility the credibility of the data. of In the the data. PDR In algorithm, the PDR thealgorithm, step estimation the step and estimation the dynamic and stepthe dynamic length model step are length improved model to are obtain improved pedestrian’s to obtain high precision pedestrian step’s high and step precision length. step The and heading step estimationlength. The is heading the core estimation of the PDR is algorithm,the core of whosethe PDR subtle algorithm, accumulation whose subtle will seriously accumulation affect will the seriously affect the accuracy of pedestrian positioning. In heading estimation algorithm, the complementary algorithm is firstly used to fuse magnetometer, accelerometer, and gyroscope to suppress the accumulated errors. Then robust adaptive Kalman filter algorithm is proposed, which can identify and suppress the outliers to improve the stability and accuracy of the positioning system. Finally, the experimental platform with inertial sensors as is established, and the experimental results demonstrate the feasibility and effectivity of the proposed PDR positioning system. Future research

Sensors 2019, 19, 294 19 of 21 accuracy of pedestrian positioning. In heading estimation algorithm, the complementary algorithm is ﬁrstly used to fuse magnetometer, accelerometer, and gyroscope to suppress the accumulated errors. Then robust adaptive Kalman ﬁlter algorithm is proposed, which can identify and suppress the outliers to improve the stability and accuracy of the positioning system. Finally, the experimental platform with inertial sensors as is established, and the experimental results demonstrate the feasibility and effectivity of the proposed PDR positioning system. Future research will be conducted to study the body kinematics model to improve the PDR algorithm. In addition, the accuracy of the PDR-based algorithm is closely related to the location of the MEMS-IMU is the focus of future research.

Author Contributions: Conception and supervision, Q.F. and P.P.; Feasibility Analysis, H.Z. and J.J.; Material purchase, Z.Z., G.Z. and Y.T.; Algorithms and software, Q.F. and P.Z.; Experiments and discussion, all authors; Original draft, H.Z.; Modification, Q.F. and P.P.; Translation, X.Z.; Submission and contact, H.Z. Funding: This research was supported in part by Six Talent Peaks Project in Jiangsu Province (No. GDZB-138), the 111 Project (B12018), National Natural Science Foundation of China (51405198). In addition, this work was supported in part by National First-class Discipline Program of Food Science and Technology (JUFSTR20180302) and Research collaboration project in Jiangsu Province (2018-178). Conflicts of Interest: The authors declare no conflict of interest.

References

1. Kim, J.W.; Kim, D.H.; Jang, B. Application of Local Differential Privacy to Collection of Indoor Positioning Data. IEEE Access 2018, 6, 4276–4286. [CrossRef] 2. Xing, B.; Zhu, Q.; Pan, F.; Feng, X. Marker-Based Multi-Sensor Fusion Indoor Localization System for Micro Air Vehicles. Sensors 2018, 18, 1706. [CrossRef][PubMed] 3. Xu, Y.; Yu, H.; Zhang, J. Fusion of inertial and visual information for indoor localisation. Electron. Lett. 2018, 54, 850–851. [CrossRef] 4. Jiang, B.; Wang, K. Algorithm Design of Low Cost Vehicle Integrated Navigation in GPS Failure. J. Transduct. Technol. 2017, 30, 412–417. 5. Xu, Y.; Shmaliy, Y.S.; Li, Y.; Chen, X. UWB-Based Indoor Human Localization with Time-Delayed Data Using EFIR Filtering. IEEE Access 2017, 5, 16676–16683. [CrossRef] 6. Alvarez, Y.; Heras, F.L. ZigBee-based Sensor Network for Indoor Location and Tracking Applications. IEEE Lat. Am. Trans. 2016, 14, 3208–3214. [CrossRef] 7. Tan, J.; Fan, X.; Wang, S.; Ren, Y. Optimization-Based Wi-Fi Radio Map Construction for Indoor Positioning Using Only Smart Phones. Sensors 2018, 18, 3095. [CrossRef] 8. De Blasio, G.; Quesada-Arencibia, A.; García, C.R.; Rodríguez-Rodríguez, J.C.; Moreno-Díaz, R. A Protocol-Channel-based Indoor Positioning Performance Study for Bluetooth Low Energy. IEEE Access 2018, 6, 33440–33450. [CrossRef] 9. Ma, Y.; Wang, B.; Pei, S.; Zhang, Y.; Zhang, S.; Yu, J. An Indoor Localization Method Based on AOA and PDOA Using Virtual Stations in Multipath and NLOS Environments for Passive UHF RFID. IEEE Access 2018, 6, 31772–31782. [CrossRef] 10. Geng, L. Design of Machine Vision Positioning System for Indoor Mobile Robots. Autom. Instrum. 2017, 38, 49–52. 11. Yang, H.; Li, W.; Zhang, H.; Gu, Y.; Fan, M. Research on fault-tolerant combined positioning technology based on SINS/UWB in complex environment. J. Sci. Instrum. 2017, 9, 2177–2185. 12. Wang, S. Application Research of ZigBee-based Wireless Sensor Network in Indoor Positioning System; North China Electric Power University: Beijing, China, 2008. 13. Tian, X.; Chen, J.; Han, Y.; Shang, J.; Li, N. Pedestrian navigation system using MEMS sensors for heading drift and altitude error correction. Sens. Rev. 2017, 37, 270–281. [CrossRef] 14. Ahmed, H.; Tahir, M. Accurate Attitude Estimation of a Moving Land Vehicle Using Low-Cost MEMS IMU Sensors. IEEE Trans. Intell. Transp. Syst. 2017, 18, 1723–1739. [CrossRef] 15. Yu, C.; Lan, H.; Gu, F.; Yu, F.; El-Sheimy, N. A Map/INS/Wi-Fi Integrated System for Indoor Location-Based Service Applications. Sensors 2017, 17, 1272. [CrossRef][PubMed] Sensors 2019, 19, 294 20 of 21

16. Chiang, K.W.; Liao, J.K.; Tsai, G.J.; Chang, H.W. The Performance Analysis of the Map-Aided Fuzzy Decision Tree Based on the Pedestrian Dead Reckoning Algorithm in an Indoor Environment. Sensors 2016, 16, 34. [CrossRef][PubMed] 17. Zampella, F.; De Angelis, A.; Skog, I.; Zachariah, D.; Jimenez, A. A constraint approach for UWB and PDR fusion. In Proceedings of the International Conference on Indoor Positioning and Indoor Navigation 2012, Sydney, Australia, 13–15 November 2012; IEEE: Piscataway, NJ, USA, 2012. 18. Xu, Q.; Li, X.; Chan, C.Y. Enhancing Localization Accuracy of MEMS-INS/GPS/In-Vehicle Sensors Integration During GPS Outages. IEEE Trans. Instrum. Meas. 2018, 67, 1966–1978. [CrossRef] 19. Jiang, W.; Li, Y.; Rizos, C.; Cai, B.; Shangguan, W. Seamless Indoor-Outdoor Navigation based on GNSS, INS and Terrestrial Ranging Techniques. J. Navig. 2017, 70, 1183–1204. [CrossRef] 20. Ai, M.; Shi, W. Indoor Positioning Technology Based on Low Cost INS/RFID. J. Comput. Meas. Control 2016, 24, 122–125. 21. Fan, Q.; Sun, B.; Sun, Y.; Wu, Y.; Zhuang, X. Data Fusion for Indoor Mobile Robot Positioning Based on Tightly Coupled INS/UWB. J. Navig. 2017, 70, 1079–1097. [CrossRef] 22. Hsu, L.T.; Gu, Y.; Huang, Y.; Kamijo, S. Urban Pedestrian Navigation Using Smartphone-Based Dead Reckoning and 3-D Map-Aided GNSS. IEEE Sens. J. 2016, 16, 1281–1293. [CrossRef] 23. Liu, J.; Shen, Q.; Li, C.; Qing, W. Signal fusion method based on optimized KF for MEMS gyro array. Syst. Eng. Electron. 2016, 38, 2705–2710. 24. Deng, Z.A.; Wang, G.; Hu, Y.; Wu, D. Heading Estimation for Indoor Pedestrian Navigation Using a Smartphone in the Pocket. Sensors 2015, 15, 21518–21536. [CrossRef][PubMed] 25. Jin, Y.; Toh, H.S.; Soh, W.S.; Wong, W.C. A robust dead-reckoning pedestrian tracking system with low cost sensors. In Proceedings of the 2011 IEEE International Conference on Pervasive Computing and Communications, Seattle, WA, USA, 21–25 March 2011; IEEE: Piscataway, NJ, USA, 2011; pp. 222–230. 26. Martinelli, A.; Gao, H.; Groves, P.D.; Morosi, S. Probabilistic Context-aware Step Length Estimation for Pedestrian Dead Reckoning. IEEE Sens. J. 2017, 18, 1600–1611. [CrossRef] 27. Khalifa, S.; Hassan, M.; Seneviratne, A. Adaptive pedestrian activity classification for indoor dead reckoning systems. In Proceedings of the 2013 International Conference on Indoor Positioning and Indoor Navigation, Montbeliard-Belfort, France, 28–31 October 2013; IEEE: Piscataway, NJ, USA, 2013; pp. 1–7. 28. del Rosario, M.B.; Redmond, S.J.; Lovell, N.H. Tracking the Evolution of Smartphone Sensing for Monitoring Human Movement. Sensors 2015, 15, 18901–18933. [CrossRef][PubMed] 29. Liu, Y.; Ren, M.; Wang, P.; Guo, H. Indoor Pedestrian Map Assisted Location Method Based on Track Estimation. J. Surv. Map. Sci. Technol. 2017, 34, 451–454. 30. Judd, T.; Levi, R. Dead Reckoning Navigational System Using Accelerometer to Measure Foot Impacts. U.S. Patent 5,583,776, 10 December 1996. 31. Sun, W.; Wang, F.; Wang, Y.; Yang, Y. Design of indoor inertial positioning system for Android mobile terminal. J. Navig. Pos. 2018, 6, 91–96. 32. Zhao, X.; Hu, A.; Zhao, W. Indoor positioning system for Bluetooth and map-assisted pedestrian dead reckoning. J. Surv. Mapp. 2016, 41, 53–58. 33. Li, S.; Cai, C.; Wang, Y.; Qiu, X.; Huang, Y. Mobile phone indoor positioning system based on geomagnetic fingerprinting and PDR fusion. J. Transduct. Technol. 2018, 31, 36–42. 34. Xu, L.; Zheng, Z.; Sun, L.; Huo, M. Multi-sensor fusion PDR localization method based on neural network. J. Transduct. Technol. 2018, 31, 579–587. 35. Zhang, T.; Chen, L.; Yan, Y. Underwater Positioning Algorithm Based on1 SINS/LBL Integrated System. IEEE Access 2018, 6, 7157–7163. [CrossRef] 36. Abbasi-Kesbi, R.; Nikfarjam, A. A Miniature Sensor System for Precise Hand Position Monitoring. IEEE Sens. J. 2018, 18, 2577–2584. [CrossRef] 37. Hsu, Y.L.; Wang, J.S.; Chang, C.W. A Wearable Inertial Pedestrian Navigation System with Quaternion-Based Extended Kalman Filter for Pedestrian Localization. IEEE Sens. J. 2017, 17, 3193–3206. [CrossRef] 38. Ibarra-Bonilla, M.N.; Escamilla-Ambrosio, P.J.; Ramirez-Cortes, J.M.; Vianchada, C. Pedestrian dead reckoning with attitude estimation using a fuzzy logic tuned adaptive kalman filter. In Proceedings of the 2013 IEEE Fourth Latin American Symposium on Circuits and Systems (LASCAS), Cusco, Peru, 27 February–1 March 2013; IEEE: Piscataway, NJ, USA, 2013; pp. 1–4. Sensors 2019, 19, 294 21 of 21

39. Hu, F.; Lü, T.; Bao, Y. Application of improved adaptive Kalman filtering in SINS/GPS integrated navigation. Comput. Eng. Appl. 2018, 54, 253–257. 40. Zhang, H.; Zhu, J.; Huang, Z. Research on Improved Adaptive Complementary Filtering Algorithm Based on AHRS. Res. Prog. Solid State Electron. 2018, 37, 157–160. 41. Sun, W.; Wen, J.; Zhang, Y.; Geng, S. Identification and Noise Reduction Method of MEMS Gyroscope Random Error. J. Electron. Meas. Instrum. 2017, 31, 15–20. 42. Li, S.; Zhou, Y.; Zhou, Y. Application of Adaptive Wavelet Threshold Denoising Algorithm in Low-altitude Flight Acoustic Target. J. Vib. Shock 2017, 36, 153–158. 43. Wang, Y.; Cai, C.; Li, S.; Yu, H. Research on indoor positioning algorithm based on pedestrian trajectory estimation. Electron. Technol. Appl. 2017, 43, 86–89. 44. Sun, J.; You, Y.; Fu, Z. Attitude Solution Method Based on Adaptive Explicit Complementary Filtering. Meas. Control Technol. 2015, 34, 24–27. 45. Sun, J.; Xu, X.; Liu, Y.; Zhang, T.; Li, Y. FOG random drift signal denoising based on the improved AR model and modified Sage-Husa adaptive Kalman filter. Sensors 2016, 16, 1073. [CrossRef][PubMed]

© 2019 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).