Research on PF-SLAM Indoor Pedestrian Localization Algorithm Based on Feature Point Map

Transcription

1 micromaches Article Research on PF-SLAM Indoor Pedestrian Localization Algorithm Based on Feature Pot Map Jgjg Shi 1,2,3, Mgrong Ren 1,2,3, * ID, Pu Wang 1,2,3 Juan Meng 1,2,3 1 College Automation, Faculty Information Technology, Beijg University Technology, Beijg 1124, Cha; shijgjg123@ s.bjut.edu.cn (J.S.); wangpu@bjut.edu.cn (P.W.); S @ s.bjut.edu.cn (J.M.) 2 Engeerg Research Center Digital Community, Mistry Education, Beijg 1124, Cha 3 Beijg Key Laboratory Computational Intelligence Intelligent Systems, Beijg 1124, Cha * Correspondence: renmgrong@bjut.edu.cn; Tel.: Received: 28 April 218; Accepted: 23 May 218; Published: 28 May 218 Abstract: Recently, map matchg-assisted positiong method based on micro-electromechanical systems (MEMS) ertial devices has become a research hotspot for door positiong; however, se are based on existg door electronic maps. In this paper, without prior knowledge map through buildg an door ma path feature pot map combed with simultaneous localization map buildg (SLAM) particle filter (PF-SLAM) algorithm idea, a PF-SLAM door location algorithm based on a feature pot map was proposed through ertial measurement unit to improve door positiong accuracy. Aimg at problem accurate headg angle estimation dead reckong (PDR) algorithm, a turn-straight-state threshold detection method was proposed that corrected difference headg angles durg straight-le walkg s to suppress error accumulation headg angle. Aimg at particles that are severely divergent at corners, a feature pot matchg algorithm was proposed to correct position error. Furrmore, turng pot extracted ma path that failed to match current feature pot map as a new feature pot was added to update map. Through mutual modification SLAM an ertial navigation system (INS) long-time, high-precision, low-cost positiong functions door s were realized. Keywords: door localization; ertial navigation system (INS); simultaneous localization map buildg (SLAM) algorithm; particle filterg; feature pot matchg 1. Introduction With rapid development modern digital formation rise smart cities, location based service (LBS) has attracted creasg attention [1]. From daily outdoor positiong navigation development to complex door environments, dem for door positiong navigation systems is high, for example, guidance shoppg malls or hospitals, location dex airports or parkg lots, safe rescue or pressional needs fields [2]. Typical door positiong technologies maly clude door positiong based on wireless networks [3,4], ertial navigation system (INS) based dead reckong (PDR) [ 7], positiong based on radio frequency (RF) signals [8,9], positiong technology based on geomagnetic matchg [1], various combations positiong technologies. Among m, INS solves difficulty global positiong system (GPS) door positiong can perform door positiong systematically dependently without relyg on an external source. The basic workg prciple this system is based on Newton s classical mechanics law, Micromaches 218, 9, 267; doi:1.339/mi

2 Micromaches 218, 9, whereby it measures acceleration formation motion carrier ertial reference coordate system, tegrates its formation with time, n converts it to navigation coordate system through quaternion or coordate transformation methods where velocity formation, yaw angle formation, position formation navigation coordate system can be obtaed [11]. The advantage is that user can position navigate by wearg a wearable sensor, given small size se sensors, re is no problem convenient carryg. However, sce navigation formation is generated by time tegration, as sensor itself creates measurement errors, positiong errors will crease with time, leadg to poor long-term positiong accuracy. Especially given errors gyroscopes, re are no reliable observations, causg headg errors to accumulate positiong failure. How to suppress accumulation headg errors is a problem that needs to be solved urgently based on INS s dead reckong system. In [12], accumulation error was elimated by headg formation before after passg through same site twice; experimenter must pass through same site twice a test, so re are also errors detection same location. The feature pot detection assisted map matchg algorithm has been embraced by researchers. Most map matchg algorithms based on feature pot detection are based on masterg prior knowledge map formation [13], that is, by selectg pots with more obvious features from buildg plane formation map as feature pots [14]. The feature pots serve as a reference to correct positiong error. However, cases emergency rescue or door maps that are difficult to obta such as fire rescue, military rescue, so on, positiong based on door maps cannot be effectively implemented. The simultaneous localization map buildg (SLAM) algorithm origatg from a robot does not need to prepare environment map formation advance can achieve function synchronous positiong composition [1]. This algorithm has advantage relatively simple external sensors that obta environmental formation does not require a priori map formation. The SLAM algorithm based on particle filterg is referred to as PF-SLAM [16]. As a non-lear filterg algorithm based on Bayesian estimation, particle filterg method has unique advantages dealg with parameter estimation state filterg problems non-gaussian non-lear time-varyg systems, which makes particles eventually converge to real pot posterior probability. We tegrate observation to formulate likelihood function, which effectively updates weights particles. Thus, particle effectiveness is enhanced to avoid particle degeneracy problem improve localization accuracy [17]. At same time, particle filter method has been successfully widely used creation concurrent maps for robot positiong door positiong system based on map matchg. The particle filter algorithm has become most important effective method for simultaneous positiong map construction mobile robots [18]. In absence prior knowledge door maps, this paper proposes a PF-SLAM door location algorithm based on feature pot maps by extractg feature pots to construct feature pot map, proposes a turn-straight-state threshold detection method to correct headg angle difference to suppress headg error accumulation. In combation with SLAM idea, through feature pot matchg algorithm, problem particle divergence at turngs due to accuracy headg angle error model was solved. Fally, re-samplg algorithm prevented particles from beg exhausted, resultg consistent calculation results algorithm, reby improvg accuracy door positiong. The system description is troduced Section 2. Section 3 gives details feature pot map. The proposed PF-SLAM algorithm, presented Section 4, is tested real scenarios results are summarized Section. 2. System Description The door positiong system proposed this paper is maly divided to four parts that are based on followg: ertial navigation algorithm Kalman filter zero velocity

3 Micromaches 218, 9, Micromaches 218, 9, x FOR PEER REVIEW 3 19 update (ZUPT) algorithm, map construction update, particle filter to predict position, SLAM position, corrects SLAM corrects positiong positiong formation formation output. Figure output. 1 shows Figure 1 shows system architecture. system architecture. Inertial sensor Inertial devices sensor such devices as three-axis such as three-axis gyros gyros accelerometers accelerometers ertial measurement ertial measurement unit (IMU) unit transmit (IMU) transmit sampled data sampled to data computer to for computer ertialfor system ertial strapdown system strapdown calculations, calculations, a Kalman filter a Kalman is troduced filter is troduced to ZUPT techniques to ZUPT [19] techniques to suppress [19] ertial to suppress device ertial errors. device On errors. basis On corrected basis position corrected attitude position output, attitude feature pot output, detectionfeature is troduced pot detection to construct is troduced origalto feature construct pot map. origal At feature same time, pot we map. extracted At same steptime, length we extracted headg for step each length zero speed, usg headg a particle for each filter zero algorithm speed, based usg ona particle filter deadalgorithm reckongbased to predict on next position dead reckong. to predict In next particle position filter algorithm,. a turn-straight-state In particle threshold filter algorithm, detection a method turn-straight-state was troduced threshold to correct detection headg method was troduced s to correct straight-le headg walkg phase, s suppressstraight-le headgwalkg cumulative phase, error, suppress predict headg headg cumulative posture error, predict headg after correction posture through extracted after turng correction pots through extracted order turng to perform pots feature pot matchg. order If itto matches perform feature existgpot feature matchg. pot database, If it matches it updates existg particle feature weight pot database, particle distribution it updates toparticle update weight particle position; distribution if it cannot to update match existg feature position; pot if it database, cannot match unmatched existg turng feature pots on database, ma path unmatched will be added turng to pots feature on pot ma mapath as a will newbe feature added pot to to exp feature pot update map as map. a new Pedestrian feature pot posesto areexp corrected through update feature map. pot Pedestrian matchg; poses at same are corrected time, through correctedfeature pot poses matchg; are used at to supplement same time, update corrected feature pot poses mapare used orderto supplement obta more accurate update feature poses pot map feature pot order map to obta formation more accurate after correction. poses feature pot map formation after correction. Data collection Feature Pot Map Inertial solution Three-axis gyro output Three-axis accelerometer output Map buildg updates Kalman velocity filter+zero speed correction errors estimati on Strapdown solution Extract step headg for each zero speed position attitude Feature pot detection Add new feature pots to update map Particle filter + headg correction position attitude Predict position Feature pot matchg Particle filter Update particle weights distributions Y match N Unmatched turng pots on ma path position correction SLAM output Update position SLAM completed? N Y Output positiong formation Figure1. 1. System architecture. 3. Feature Pot Map 3.1. Feature Pot Detection Extraction The ertial navigation positiong system can dependently perform positiong without relyg on or signal sources, accuracy a short time is very high. Therefore,

4 Micromaches 218, 9, Feature Pot Map 3.1. Feature Pot Detection Extraction The ertial navigation positiong system can dependently perform positiong without Micromaches 218, 9, x FOR PEER REVIEW 4 19 relyg on or signal sources, accuracy a short time is very high. Therefore, trajectory trajectory a short time a short can reflect time can part reflect part formation formation terior buildg terior map. buildg With map. With aid se aid characteristics se characteristics ertial navigation, ertial navigation, experiment experiment was designed was without designed door without map door formation, map formation, s straight s walkg phase straight waswalkg detected phase from was detected path from output by short-time path ertial output navigation. by short-time The pot ertial tersection navigation. between The pot adjacent tersection les is between feature pot adjacent required les is by this feature study. pot required by this study. In order order to to clarify clarify difference difference connection connection between between feature feature pots pots this this article article turng turng pots pots s s walkg walkg process process describe describe advantages advantages correctg correctg positions bypositions extractg by such extractg featuresuch pots feature stead pots stead turng pots turng as benchmarks, pots as benchmarks, above figure above is provided. figure is Figure provided. 2a shows Figure a2a part shows a part experiment experiment site. The site. yellow The yellow arc arc figure isfigure more is common more common turng trajectory turng trajectory a a passg through passg through tersection. tersection. Or possible Or trajectories possible are trajectories shown blue are shown dotted les blue dotted Figureles 2b, where Figure one 2b, where biggest one radian parts biggest asradian defedparts as as defed turng as pot turng, pot is, shown as is shown blue square. as blue Fromsquare. position From position blue square, blue it cansquare, be seen it can thatbe seen position that position passg through passg through corner has corner romness. has romness. Combed Combed with actual with actual walkg characteristics, walkg characteristics, s s walkg walkg normal straight-le normal straight-le walkg phase walkg are relatively phase are regular. relatively Therefore, regular. Therefore, straight-le straight-le fittg s fittg s can be performed can be performed by extractg by extractg s s straight-le straight-le walkg phases, walkg asphases, shown as shown green ellipse green circled ellipse Figure circled 2b. Figure The extension 2b. The extension le segment le issegment extendedis to extended one pot, to one as shown pot, as shown red square red Figure square 2b. This Figure pot 2b. isthis pot feature is pot feature to be detected pot to be detected extracted extracted this paper. From this paper. Figure From 2b, it can Figure be seen 2b, it that can such be seen feature that pots such feature are more pots suitable are more for suitable correction for correction position than position than turng pot. turng A number pot. feature A number pots feature are fitted pots to a plurality are fitted to a plurality trajectories thattrajectories pass through that pass tersection through fromtersection same startg from pot same to startg obta a pot plurality to obta feature a plurality pots. This feature position pots. can be This seen position as tersection can be seen as terior tersection path buildg. terior path This explanation buildg. is more This conducive explanation to is understg more conducive buildg to understg feature pot buildg map below. feature pot map below. Turng pots Feature pots Lear fittg section (a) (b) Figure Figure (a) (a) Part Part experiment experiment site site (b) (b) schematic schematic diagram diagram fittg fittg straight straight walkg walkg phase. phase. The headg angle is most important angle formation attitude angle formation. The headg angle is most important angle formation attitude angle formation. The change headg angle formation was directly used [12,2] to detect -related The change headg angle formation was directly used [12,2] to detect -related walkg status. However, durg experiment, sensor was fixed on waist walkg status. However, durg experiment, sensor was fixed on waist experimenter, experimenter, headg angle formation could not occur due to large fluctuations durg straight walkg phase. The experiments this paper placed IMU sensor fixed on foot, sce s cannot always ensure that toe is always parallel to walkg direction even when walkg a straight le, so headg angle formation output by sensor will appear as larger fluctuations even walkg phase, which can lead to false detections, refore it is impossible to observe this effectively a straight le.

5 In order to extract straight-le walkg phase with better learity from position data correspondg to trajectory derived from dead-reckong, a feature pot extraction method based on position formation was proposed. In experiment, mean all samplg pots 1 s was taken accordg to Equation (2). On one h, it avoids error some samplg pots avoids impact feature detection; on or h, difference adjacent data is creased, which is easy to observe be furr treated. The samplg frequency was set to 1 Hz this article s experiments. Micromaches 218, 9, headg angle formation could not occur due to large fluctuations durg straight walkg phase. The experiments this paper placed IMU sensor fixed on foot, sce s cannot always ensure that toe is always parallel to walkg direction even when walkg a straight le, so headg angle formation output by sensor will appear as larger fluctuations even Micromaches 218, 9, x FOR PEER REVIEW 19 walkg phase, which can lead to false detections, refore it is impossible to estimated observe this step effectively length a straight combes le. headg formation obtaed from gyro magnetic sensors The[21]. MEMS-based Usg Equation PDR positiong (1) to calculate method is current basedposition on physiological. characteristics gait s. It uses accelerometer to measure number walkg steps estimated step length combes headgformation Xt = Xt 1 + obtaed Ltcosαt from gyro magnetic sensors [21]. (1) Usg Equation (1) to calculate current Yposition. t = Yt 1 + Ltsαt { where ( X Xt = X t 1 + L t cos α t t, Y t) are coordates position at time t ; L t is pace (1) Y t = Y t 1 + L t s α at time t ; α t t is difference headg angle at moment where (Xt,. Y t ) are coordates position at time t; L t is pace The at timeposition t; αcalculated t is difference by ertial headg navigation anglesystem based on at moment dead t. reckong The is gradually position calculated calculated based by on ertial s navigation position system at based last moment, on as well dead as reckong step is gradually difference calculated headg based onangle s at that moment. position Therefore, at last when moment, a as wellwalks as step a straight le, difference even if headg angle fluctuates that moment. at a certa Therefore, moment when resultg a a deviation walks a straight le, even position, if headg for a period angle fluctuates time, attrajectory a certa moment resultg a deviation straight-le walkg stage position, still exhibits for a period a lear fittg time, characteristic. trajectory straight-le walkg stagein still order exhibits to expla a lear this fittg problem characteristic. more clearly, followg uses a test experiment to illustrate process In order teature explapot this problem detection more clearly, extraction. followg Pedestrians uses beg a test at experiment startg topot illustrate walk process counterclockwise feature pot along detection ma door extraction. path. The Pedestrians walkg beg trajectory at is startg rectangular. potas shown walk counterclockwise Figure 3a below, along purple ma dot is door startg path. The pot, walkg trajectory direction is rectangular. arrow is As shown direction Figure. 3a below, Due purple to dot short is distance startg pot, short time, direction ertial navigation arrowoutput is direction position formation. is more Dueaccurate. to short Figure distance 3b is short headg time, angle ertial formation navigationdiagram output position correspondg formation is more IMU accurate. output Figure durg 3b is headg walkg. angle formation The blue rectangle diagram Figure correspondg 3 is correspondg IMU output durg trajectory walkg. formation The blue rectangle headg Figure angle 3data is formation. correspondg It can be seen that trajectory headg formation angle data headg will still angle show data fluctuations formation. even It can if be seen that is headg a straight-le angle data walkg will still show phase. fluctuations It is not possible even if to identify effectively is awher straight-le walkg phase. is It isstraight-le not possible walkg to identify phase, effectively but wher trajectory isderived straight-le from s walkg phase, dead-reckong but still present trajectory lear derived fittg from characteristics. s dead-reckong still present lear fittg characteristics. Y Position/m Yaw[ ] (a) Measurg pots (b) Figure 3. (a) Pedestrian trajectory ertial navigation system (INS) output (b) correspondg Figure 3. (a) Pedestrian trajectory ertial navigation system (INS) output (b) correspondg headg angle data durg walkg. headg angle data durg walkg.

6 Micromaches 218, 9, In order to extract straight-le walkg phase with better learity from position data correspondg to trajectory derived from dead-reckong, a feature pot Micromaches 218, 9, x FOR PEER REVIEW 6 19 extraction method based on position formation was proposed. In experiment, mean all samplg pots 1 s was taken accordg to Equation (2). On one h, it avoids error some f f samplg pots avoids impact feature detection; on or h, difference adjacent data is creased, which is easy Xi Y to observe 1be1i furr treated. The samplg frequency i= i= (2) was set to 1 Hz this article s experiments. X m = () t, Ym = () t f f In formula, X m is mean value X abscissa i Y all samplg pots 1 s at time t. i i=1 i=1 X i is abscissa all samplg X m pots = (t), 1 s. Y m Y = (t) (2) f m is mean value ordate all f samplg pots 1 s at time t. Y i is abscissa all samplg pots 1 s. f is samplg In frequency. formula, X m is mean value abscissa all samplg pots 1 s at time t. X i is abscissa Afterwards, all samplg adjacent pots average 1 s. samplg Y m is mean pot value is calculated ordate accordg all to samplg Equation pots (3) to obta 1 s at time t. connection Y i is abscissa angle, all samplg Figure 4a pots is obtaed. 1 s. f is After samplg data frequency. is smood to obta Figure Afterwards, 4b, adjacent has average a static samplg terval pot before is startg calculated to accordg walk to measure to Equation (3) drift to obta gyro s connection constant value. angle, Therefore, Figure 4a first is obtaed. segment After figure data is is drift smood measurement to obta phase Figure 4b, is not cluded has a calculation static terval as shown before by startg part to walk side to measure blue rectangle drift Figure gyro s 4b. It constant can be clearly value. Therefore, seen from first figure segment that four-segment figure is straight-le drift measurement walkg phase, phase order is not to cluded obta a better fittg calculation effect, as took shown out by mid-section part side data blue rectangle straight-le Figure 4b. It walkg can be clearly phase seen performed from figure straight-le that fittg four-segment as shown straight-le by part walkg side phase, yellow rectangle. order to obta a better fittg effect, took out mid-section data straight-le walkg phase performed 1i i straight-le fittg as shown by part side yellow rectangle. i Ym Y m θ =arctan 1 ( i i ) θ i = X Y mi arctan Y X m i 1 m (3) i Xm X i m i 1 (3) -π < θ π π < θ i π 1In formula, ( i i i i 1In formula, ( X Xm, i m, Y Ym) i m ( ) X i 1 ( Xm, Y m, Ym i 1 ) m are ) are coordates adjacent two mean coordates adjacent two mean samplg pots; θ i i samplg pots; is connectg θ is angle connectg angle adjacent mean adjacent samplg mean pots. samplg pots. f f Angle[ ] Angle[ ] Measurg pots (a) Measurg pots (b) Figure Figure (a) (a) Unsmood Unsmoodangle angleformation (b) (b) smood smood angle angle formation. formation. Use first-order le fittg straight-le walkg phase extracted by Equation (4) Use first-order le fittg straight-le walkg phase extracted by Equation (4) obta fitted straight le: obta fitted straight le: n ˆb = XY ˆ i i i nxy = n i=1 X iy i nxy i=1 b = 22n 1 1n X2 i nx2 â = YX i i nx = ˆbX (4) p = â + ˆbq (4) aˆ = Y bx ˆ p= aˆ + bq ˆ

7 Micromaches 218, 9, x FOR PEER REVIEW 7 19 where (, ) X Y are coordates mean position pot for extracted fittg part; Micromachesi218, i 9, (, ) X Y is mean coordate all samplg pots representg fitable parts extracted; where (X i, Y i ) are coordates mean position pot for extracted fittg part; ( X, Y ) is n is mean total coordate number all samplg samplg pots pots representg fitable part fitable extracted. ˆb parts extracted; is coefficient n is total number one-time term samplg for pots fitted straight fitable le. part â extracted. is coefficient ˆb is coefficient lear constant one-time term; term for p is fitted straight-le le. expression. â is coefficient lear constant term; p is straight-le expression. The feature pots extraction method based on location formation is is shown Figure. The blue le segment is straight le fitted by s straight-le walkg phase. The fittg adjacent straight-le extension le to pot is is one required for this study. The feature pots are shown by red circle figure. In order to obta more accurate feature pots, it it was necessary to to design design experiment to to take take multiple multiple values values for for same same feature feature pot pot obta obta average average value value as as fal feature fal feature pot data pot data order to order construct to construct a more a accurate more accurate feature pot feature map. pot map. Y Position/m -4-6 Startg pot Feature pots Pedestrian true track Fittg le Extendg le Figure. Feature pots extraction based on location formation Build Ma Path Feature Pot Map Most terior buildgs are made up corridors rooms. In this paper, we call corridor paths ma ma door door paths, paths, which which reflects reflects general general situation situation terior terior a buildg. a The buildg. feature The pots feature on pots ma on path ma are easier path are to observe easier to observe extract. The extract. variability The variability layout layout room is too room largeis too feature large pot feature extraction pot extraction is difficult. is Therefore, difficult. Therefore, feature pot feature extraction pot extraction construction construction updatg updatg feature pot feature map proposed pot map proposed this paper were this aimed paper at were aimed at turng potturng that occurs pot onthat occurs ma path. on ma path. Based on short-time high precision ertial navigation system that has advantages autonomy to design experiments, that is, s have autonomous obstacle avoidance, autonomously select roads that y can travel, can dependently design route-walkg characteristics. Each time experimenter started from same startg pot walked along ma path door buildg, experiment showed that shorter walkg time, time, fewer fewer number number turns, turns, higher higher precision precision INS output INS trajectory. output The trajectory. trajectory The trajectory same color same Figure color 6 was Figure an experimental 6 was an path, experimental feature path, pots were feature extracted pots accordg were extracted to a feature accordg pot to extraction a feature pot method extraction based on method position based formation on position formation are represented are by circles represented by same circles color. When same designg color. When experimental designg path, experimental it was ensured path, that it was tersection ensured that each tersection path passed ateach leastpath twice, passed fally at least twice, average value fally was taken average as value required was feature taken as pot. As required each path feature started pot. from As each same path startg started pot, from suchsame as startg purplepot, dots such Figure as 6, purple paths dots were put Figure toger 6, paths combed were put with toger extracted combed feature pots with to build extracted a feature feature pot pots map that to build coulda reflect feature pot ma map path that could buildg. reflect ma path buildg.

8 Micromaches 218, 9, Micromaches 218, 9, FOR PEER REVIEW Micromaches 218, Micromaches 9, x FOR PEER 218, REVIEW 9, x FOR PEER REVIEW Y Position/m -4-6 Y Position/m Position/m Figure 6. Feature Figure pots Feature map. pots map. In order to verify accuracy feature pot map, maps were matched with In order to In verify order to accuracy verify accuracy feature pot feature map, pot maps map, were maps matched were with matched with experimentally selected door architectural map. The matchg result is shown Figure 7. From experimentally experimentally selected door selected architectural door architectural map. The matchg map. map. The The result matchg is shown result result is shown is Figure shown 7. Figure From Figure 7. From 7. From figure, it can be seen that extracted feature pots can be seen as cross pot route figure, it figure, can figure, be itseen canit be can that seen be seen that extracted that extracted feature extracted feature pots feature pots can be pots can seen becan as seen a be cross aseen a cross pot as a pot cross pot route route with route with allowable error range, constructed feature pot map can reflect approximate with allowable with allowable error allowable error range, range, error range, constructed constructed constructed feature feature pot pot feature map map can pot reflect canmap reflect can approximate reflect approximate path path terior buildg. The details two zoomed- feature pots are as shown left path terior path terior buildg. terior buildg. The buildg. details The details The details two zoomed- two zoomed- two feature zoomed- feature pots feature pots are as are pots shown as shown are as shown left left sideleft side picture correspondg to pots B, respectively. At pot A, passes side picture side picture correspondg picture correspondg to pots to pots A to Apots B, respectively. B, A B, respectively. At pot At pot A, At a A, pot a A, a passes passes through passes through tersection twice. Each time extracted feature pots are shown different through through tersection tersection twice. twice. Each Each twice. time time Each extracted time feature extracted pots feature are shown pots are shown different color different circles, color circles, average two feature pots is output to obta feature pots required by color circles, color average circles, average two feature pots two feature is output pots to obta is output to feature obta pots feature required pots by required this article, by this article, as shown black Figure 7. Pot is same as pot A: this article, as this shown article, as black shown black Figure 7. Pot Figure B is7. Pot same B is as pot same A: as pot A: passes passes tersection three times. passes tersection passes tersection three times. three times pot zoom A pot zoom A pot zoom pot zoom 18 2 B pot zoom B pot zoom Y Position/m -1 Y Y Position/m A A B B Position/m Figure 7. Matchg feature pot maps with terior buildg plans details two Figure 7. Matchg Figure feature Matchg pot feature maps with pot terior maps with buildg terior plans buildg plans details details two two zoomed- feature pots. zoomed- feature zoomed- pots. feature pots. The feature pot map not only contas extracted feature pot formation, but also The feature The pot feature map not pot only map contas not only contas extracted feature extracted pot feature formation, pot formation, but also but also cludes The feature position potformation map not only contas each pot on extracted ma feature path, reby pot formation, distguishg but also wher cludes cludes cludes position formation position formation each pot on each pot ma on path, reby ma path, distguishg reby distguishg wher wher positionturng formation pot occurs each pot on onma ma path path, or side reby distguishg house, which wher provides ideas for turng pot turng occurs pot on occurs ma on path or ma side path or house, side which house, provides which ideas provides for ideas for followg turng pot new occurs feature onpot ma extraction. path or That sideis, this house, position which formation provides ideas constitutes for followg ma path new followg new followg feature new pot feature extraction. pot That extraction. is, this That position is, this formation position formation constitutes constitutes ma path ma path database, feature pot extraction. extracted That new is, this turng position pot formation is matched constitutes with ma path madatabase path database, one by one. If database, database, extracted new extracted turng new pot turng is matched pot with is matched ma with path database ma path one database by one. If one by one. If matchg distance is less than given threshold, it is determed that turng pot occurs on matchg distance matchg is less distance than a is given less than threshold, a given it threshold, is determed it is that determed turng that pot turng occurs pot on occurs on ma path. Judgg by Equation (): ma path. Judgg ma path. by Equation Judgg by (): Equation ():

9 Micromaches 218, 9, extracted new turng pot is matched with ma path database one by one. If matchg distance is less than a given threshold, it is determed that turng pot occurs on ma path. Judgg by Equation (): ( ) d n i (x T = n T x i ) 2 ( l + y n T y i 2 l) { ( ) 1 d l n n i T = T Th () ors In formula, ( x n T, yn T) is coordate nth turn pot extracted; ( x i l, y i l) is coordate ith position ma path database; ( d n i T) is distance between extracted nth turn pot ith position pot ma path database. Th is threshold value, which is determed for turng pot state, specific value needs to be determed accordg to actual situation experimental site. l n T = 1 dicates extracted nth turn pot that occurs on ma path. l n T = dicates that extracted nth turn pot is not on ma path. 4. Design Particle Filter-Simultaneous Localization Map Buildg (PF-SLAM) Indoor Pedestrian Positiong System Based on Feature Pot Map 4.1. SLAM Problem Description Simultaneous localization mappg, abbreviated as SLAM, means that when a movg object is completely unfamiliar to its environment, its location is not determed, such as emergency rescue missions, for example, fire rescue. In this case, by creatg a surroundg environment map n usg created environment map to help user to achieve ir own positiong, SLAM maly solves two major problems. These two problems are mutually premised teract with each or: first, an environment map is constructed. This is achieved by collectg formation about surroundg environment through various sensors worn on carrier. However, this formation is ten large unorganized, feature formation needs to be extracted from this formation aggregated to an environment map. Second, user s positiong determes location user constructed environment map. The SLAM idea is core idea entire positiong system. The ma implementation steps SLAM algorithm can be summarized followg steps: (1) Feature pots are extracted, ma path feature pot map is constructed. The INS system obtas formation location, accuracy which is determed by accuracy sensors samplg time. The feature pots door ma path are constructed by extractg feature pots from straight-le fittg straight walkg phase through more accurate ma path attitude formation obtaed. (2) Location prediction. The headg angle difference is corrected detected straight-le walk state, reby suppressg accumulation headg angle error. The s position posture are predicted by corrected headg particle filter algorithm based on dead reckong. (3) Feature pot matchg. Determe wher observed position is a turng pot. If yes, match feature pot update particle weight particle distribution. If it cannot be matched with current feature pot database, flection pot on ma path is extracted as a new feature pot added to feature pot map. (4) Correction. The position is corrected accordg to feature pot matchg result, detected turng pot on ma path, which when unmatched is used to exp update feature pot map formation.

10 Micromaches 218, 9, PF-SLAM System Model The SLAM system a practical project is a non-lear system where noise does not follow Gaussian distribution. If method based on extended Kalman filter-slam (EKF-SLAM) is used to estimate non-lear non-gaussian state, lear errors will evitably be troduced to system, particle filterg based on Monte Carlo sequential importance samplg (SIS) is suitable for processg non-lear non-gaussian systems. It is an effective method for processg SLAM systems. Therefore, SLAM method based on particle filterg was used this paper. Particle filterg is implemented by non-parametric Monte Carlo simulation method for Bayesian filterg. The prior a posteriori formation is described sample form stead function. The SLAM problem needs to be solved by calculatg jot posterior probability density distribution pose feature pot map: P(x 1:k, θ z 1:k, u 1:k ) (6) In formula, x 1:k = [x 1, x 2,, x k ] T represents walkg path up to time k; θ = [x 1, y 1, x 2, y 2,, x n, y n ] T dicates position feature pot feature pot map; z k is observation, note z 1:k = [z 1, z 2,, z k ] T represents observation sequence up to time k; u k is control put, note u 1:k = [u 1, u 2,, u k ] T represents control put sequence up to time k. The core idea particle filterg SLAM is to factorize jot posterior probability distribution to path part map part with path as condition. P(x 1:k, θ z 1:k, u 1:k ) = P(θ x 1:k,z 1:k, u 1:k )P(x 1:k z 1:k,u 1:k ) (7) If s entire path x 1:k is given, map θ can be calculated analytically. Therefore, posterior probability s pose P(x 1:k z 1:k,u 1:k ) can be estimated by particle filterg method. Based on this, map θ can be recursively calculated by Kalman filter. The particle importance weights are: w (i) k x (i) P(z k k )w (i) k 1 (8) where w (i) k is weight particle at moment;w (i) k 1 is weight particle at previous xk moment; P(z (i) k ) is posterior probability position. Perform particle weight normalization: w (i) k = w(i) k N w (i) k i=1 (9) where N is total number particles. Doucet [22] showed that variance weights creases with time, which leads to degeneracy problem that prevails particle filterg. An appropriate measure degradation is adoption effective populations N e f f. The smaller N e f f dicates that degradation is more serious. N e f f can be calculated by followg equation [12]: 1 N e f f = (1) N (w (i) k )2 i=1 The re-samplg method is ten used to solve degeneracy phenomenon, re-samplg is performed when number effective particles is less than a given number. Given a valid particle

11 Micromaches 218, 9, x FOR PEER REVIEW Micromaches The re-samplg 218, 9, 267 method is ten used to solve degeneracy phenomenon, re-samplg is performed when number effective particles is less than a given number. Given a valid particle threshold N threshold, resamplg is performed when number effective particles threshold N threshold, resamplg is performed when number effective particles N e f f N threshold, reby Neff Nmatag threshold, reby a consistency matag algorithm. a consistency algorithm Design PF-SLAM Localization Algorithm Based on on Feature Pot Pot Map Map Figure 8 is a flow chart PF-SLAM location algorithm based on on feature pot pot map. map. Accelerometer gyro output data are used for for ertial navigation, step headg formation for each step is obtaed through ZUPT algorithm. Particle filterg based on dead reckong predicts predicts position. position. The Thestate state s s at at moment moment is is judged judged by by turn-straight-state turn-straight-state threshold threshold detection detection method. method. If it If is ita is straight a straight walkg walkg stage, stage, headg headg angle angle difference difference is corrected, is corrected, headg headg angle angle error error accumulation accumulation is suppressed. is suppressed. If it If is it is a turng a turng pot, pot, n n feature feature pot pot matchg matchg is performed. is performed. If it Ifmatches it matches feature feature pot pot stored stored current current feature feature pot pot map, map, n n particle particleis isupdated updatedas as feature pot pot position its weight based based on on position position relationship relationship all particles all particles feature feature pots at pots moment at moment is updated, is updated, weight is normalized weight is normalized position output. position If output. turngif pot turng cannot match pot cannot existg match feature existg pot database, feature pot it does database, not update it does not weight update particle, weight it particle, is judged as to it wher is judged it belongs as to wher to turng it belongs pot to on turng ma path. pot If yes, on ma path. position If yes, data are added to position feature data are pot added database to feature feature pot pot database map to update feature pot feature map pot to update map. Fally, feature it is judged pot map. as tofally, wher it is judged particles as need to wher to be re-sampled. particles Such need a loop to be realizes re-sampled. positiong Such a loop door realizes s. positiong door s. Figure 8. Flow chart particle filter-simultaneous localization map buildg (PF-SLAM) Figure 8. Flow chart particle filter-simultaneous localization map buildg (PF-SLAM) location algorithm based on feature pot map. location algorithm based on feature pot map. In summary, ma modules system clude an ertial navigation calculation module based In on summary, ZUPT, a ma ma path modules feature pot system map clude module, an ertial a particle navigation correlation-based calculation module data based association on ZUPT, module. a ma The ertial path feature navigation pot calculation map module, module based a particle on ZUPT correlation-based provides motion data association module. The ertial navigation calculation module based on ZUPT provides motion

12 Micromaches 218, 9, formation to system; ma path feature pot map module provides system with real-time observed feature pot formation saves real-time updated feature pot map formation; data correlation module based on particle filter is core whole system. It maly suppresses accumulation headg angle error accordg to observed values system performs data association processg with feature pot map updates weight particle obtas fal matchg result. Therefore, we will focus on troduction how to suppress accumulation headg angle errors based on particle filterg how to perform feature pot matchg particle weight update The Turn-Straight-State Threshold Detection Method In actual project, although foot-tied ertial navigation system can troduce zero-speed observation when foot touches ground through ZUPT stage, n update prciple accordg to Kalman filter, acceleration drift error is reduced. However, for gyro errors, re are no reliable observations, leadg to serious drift long-distance headg errors. Therefore, this paper proposed turn-straight-state threshold detection method to suppress headg error accumulation over time. The particle filter algorithm based on dead reckong will output one-step headg angle formation at every zero speed pot. Pedestrian headg angle formation can reflect movement state to some extent. Although headg angle change model is accurate, re is significant variation difference headg angle between turng stage straight walkg stage. Therefore, this paper proposed a turn-straight-state threshold detection method to determe status. Pedestrian turng pots were matched with feature pot maps to perform position correction particle weight updatg. Pedestrians were detected straight-le walkg phase headg angle difference was corrected to suppress accumulation headg angle errors. The difference headg angle is: ϑ n = yaw n yaw n 1, n = 2, 3, m (11) where ϑ n is difference headg angle nth zero velocity pot; yaw n is headg data nth zero velocity pot; yaw n 1 is headg data n 1th zero velocity pot; ϑ n is itial headg when n = 1; m is total number steps for s durg experiment. Combed with actual situation, when walks a straight le, headg angle value remas basically unchanged, so headg angle difference was set to to suppress accumulation headg angle error. Due to s discretion when turng corner, corner angle buildg actually beg different, correspondg headg angle difference will be different will only detect turng phase without modifyg value. The correspondg settg turn-straight-state threshold detection method is as follows: θ n = { ϑn Th1 < ϑ n < Th2 ors (12) where θ n is difference corrected headg angle nth zero velocity pot; ϑ n is difference before correction headg angle nth zero velocity pot; Th1, Th2 are state judgment threshold where specific value needs to be determed accordg to accuracy IMU specific conditions experiment Feature Pot Matchg Although turn-straight-state threshold detection method can effectively suppress headg error accumulation as a walks straight, particle filter derived from Monte Carlo recursive Bayesian estimation has rom characteristics. Even though

13 Micromaches 218, 9, 9, 267 x FOR PEER REVIEW headg formation is corrected, it is evitable that re will be a serious divergence particles at headg formation is corrected, it is evitable that re will be serious divergence particles at flection pot, which will result positiong failure. For this reason, with aim flection pot, which will result positiong failure. For this reason, with aim addressg addressg problem where accuracy headg angle error model particle problem where accuracy headg angle error model particle filterg equations filterg equations causes particles to be severely divergent at corners, a feature pot causes particles to be severely divergent at corners, a feature pot matchg algorithm was matchg algorithm was proposed. Correspondg feature pot matchg through detected proposed. Correspondg feature pot matchg through detected turng pot turng pot feature pot map turng pot were matched to feature pot map turng pot were matched to relatively accurate relatively accurate feature pot particle weight was updated to ensure effective feature pot particle weight was updated to ensure effective correct propagation correct propagation particle. particle. As shown Figure 9, black particles figure are partial particles at a certa moment. As shown Figure 9, black particles figure are partial particles at certa moment. The blue dots figure are positions s obtaed by weightg all particle The blue dots figure are positions s obtaed by weightg all particle positions at that moment. If it is detected that is at turng pot at this moment, positions at that moment. If it is detected that is at turng pot at this moment, output position particle at this moment is updated to position matched output position particle at this moment is updated to position matched characteristic characteristic pot, that is, at red circle figure. The particle filter makes weight pot, that is, at red circle figure. The particle filter makes weight formation formation particles limited by feature pot formation after beg tegrated to particles limited by feature pot formation after beg tegrated to feature pot map [23]. feature pot map [23]. After feature pot matchg is successful, particle weight is updated After feature pot matchg is successful, particle weight is updated accordg to distance accordg to distance between each particle position matched feature pot at between each particle position matched feature pot at moment, updated weight moment, updated weight is assigned to next moment, so particles contue to is assigned to next moment, so particles contue to propagate forward. The particle weights propagate forward. The particle weights are updated as follows: are updated as follows: (i) w (i) () i k 1 () i wk 1 unmatched w k = xk P(z (i) k )w (i) (13) () i () i Pz ( x ) w matched () i ( k k ) k k kk 1 P(z k xk (i) ) is posterior probability position this article can be expressed as: xk P(z (i) k e (d (i) () i 1 k )2 22 Pz ( k xk ) = e (14) 2π (i) () i () i (x (i) 2 x T ) 2 () i + (y (i) 2 where d y T ) 2 k = ( xk xt) + ( yk yt) dicates distance between iith particle k = matched feature pot at time k. k. k k = Pz x is posterior probability position this article can be expressed as: ( i ) 2 ( d k ). System Experiment Result Analysis Figure 9. Feature pot matchg diagram. The system does not rely on or equipment, only relyg on IMU to collect data; navigation computer receives IMU sampled data through USB terface can be stored real time. The navigation computer adopts an ordary PC to implement ertial navigation

14 Micromaches 218, 9, System Experiment Result Analysis The system does not rely on or equipment, only relyg on IMU to collect data; navigation computer receives IMU sampled data through USB terface can be stored Micromaches 218, 9, x FOR PEER REVIEW Micromaches real time. 218, The9, navigation x FOR PEER REVIEW computer adopts an ordary PC to implement ertial navigation solution, filterg filterg algorithm, algorithm, buildg buildg feature feature pot pot maps, maps, feature feature pot matchg pot matchg or algorithm or algorithm steps, solution, filterg ultimately steps, algorithm, achieves ultimately buildg achieves door feature positiong. pot door maps, The positiong. feature numberpot The particle matchg number filtered particles or filtered algorithm experiment particles steps, was. ultimately experiment The IMU achieves was consists. The aimu three-axis consists door MEMS positiong. a three-axis gyroscopethe MEMS number a three-axis gyroscope particle MEMS a three-axis accelerometer. filtered particles MEMS The accelerometer. ertial experiment measurement The was ertial. device The measurement isconsists fixed todevice a three-axis foot IMU fixed MEMS, to gyroscope as foot shown a, Figure three-axis 1. MEMS as shown accelerometer. Figure 1. The ertial measurement IMU is fixed to foot, as shown Figure 1. Figure 1. Inertial measurement unit (IMU) fixed to experimenter s foot. Figure 1. Inertial measurement unit (IMU) fixed to experimenter s foot. Figure 1. Inertial measurement unit (IMU) fixed to experimenter s foot. In order to verify proposed PF-SLAM door location algorithm based on feature pot In map order proposed to verify this proposed paper, PF-SLAM experimental door site selected for location this study algorithm was based tenth on floor feature pot science map proposed buildg Beijg this paper, University experimental Technology. site site selected The experimental for for this this study study was area was was tenth 8 tenth floor 2 floor square meters. science science buildg The buildg concrete Beijg buildg Beijg University University formation Technology. Technology. map The experimental The experimental experimental area ideal was area 8 route was 28 are square shown 2 meters. square The Figure meters. concrete 11. The The concrete buildg purple circle buildg formation formation figure map shows map experimental startg experimental pot ideal route ideal are shown route are was Figure shown also 11. The startg Figure purple 11. pot The circle purple path circle figure shows ma figure path shows startg feature pot pot startg map, pot thus avoidg was experimental alsowas startg also error pot caused startg by pot path accuracy path ma path ma feature itial path pot positiong. feature map, pot thus The map, avoidg purple thus avoidg trajectory experimental experimental Figure error caused 11 is error by experimental caused accuracy by preset accuracy itial positiong. trajectory. itial The positiong. purple The experimenter trajectory The purple Figure started trajectory 11 with is (,) experimental Figure eventually 11 preset is returned experimental to trajectory. preset startg The experimenter pot. The trajectory. direction startedthe with experimenter (,) black eventually arrow started is returned with walkg (,) to direction startg eventually pot. The, returned direction to startg black overall arrow pot. path is shows The direction walkg a closed direction twisted black shape. arrow, is walkg overall direction path shows a, closed twisted shape. overall path shows a closed twisted shape. X Figure 11. Experimental site preset walkg path. Figure 11. Experimental site preset walkg path. In order to see effect algorithm more clearly, trajectory based on In order dead-reckoned to see effect particle filter algorithm output more was clearly, combed with trajectory door buildg based on plan. Figure 12 shows dead-reckoned results particle particle filter filter output output was without combed addg with proposed door algorithm. buildg plan. The experimental Figure 12 shows walkg results time was particle s, a total filter output 142 steps, without addg walkg distance proposed was algorithm The s experimental fal walkg position time coordate was was s, a total (2.39,2.646). 142 steps, From walkg part enclosed distance by was red ellipse m. The s figure, it can fal be position seen that coordate was (2.39,2.646). trajectory From output part showed enclosed serious by phenomenon red ellipse

15 Micromaches 218, 9, In order to see effect algorithm more clearly, trajectory based on dead-reckoned particle filter output was combed with door buildg plan. Figure 12 shows results particle filter output without addg proposed algorithm. The experimental walkg time was s, a total 142 steps, walkg distance was m. The s fal position coordate was (2.39,2.646). From part enclosed by red ellipse figure, it can be seen that trajectory output showed serious phenomenon passg through wall, positiong some locations failed. Micromaches 218, 9, x FOR PEER REVIEW 1 19 Micromaches 218, 9, x FOR 1 PEER REVIEW 1 19 Y Position/m Y Position/m Figure 12. Pedestrian -1 trajectory output 1 by 2 3 filter based 4 on 6 dead 7 reckong. 8 Figure 12. Pedestrian trajectory output by particle filter based on dead reckong. Figure 13a is difference headg angle correspondg to each zero velocity pot Figure 12. Pedestrian trajectory output by particle filter based on dead reckong. Figure 13a is difference above experiment. headg From angle figure, correspondg it can be seen to each that zero difference velocity pot headg angle cannot effectively observe state. Through turn-straight-state Figure above 13a is experiment. difference From headg figure, angle it can correspondg be seen that to each difference zero velocity pot headg threshold detection method proposed this paper, turng pot walkg angle cannot effectively observe above experiment. state From. figure, Through it can be seen turn-straight-state that difference threshold straight phase were separately analyzed; s turng pot (marked red) can be detection headg method angle proposed cannot effectively this paper, observe turng state pot. Through walkg turn-straight-state phase detected as shown Figure 13b, at same time, as detected s walked a were separately threshold detection straight le, analyzed; method difference s proposed headg turng this paper, angle pot was (marked turng set to zero pot red) to suppress can be detected headg as walkg error shown Figure straight 13b, accumulation. phase at Accordg were sameseparately time, to Equation as analyzed; detected (12) s s IMU s accuracy walked turng pot experiment a straight (marked le, conditions, red) difference can be headg detected threshold angle as was shown was set set to: to Figure zero to 13b, suppress at headg same error time, accumulation. as detected Accordg s to walked Equation a (12) straight le, difference headg angle 9was 1set 9to zero to suppress headg error IMU s accuracy experiment conditions, ϑn. < ϑthreshold n <. was set to: accumulation. Accordg to Equation (12) θn = IMU s accuracy experiment conditions, (1) threshold was set to: { ors ϑn.9< ϑ 9n < 1.919θ n = (1) 4 ϑnors. < ϑ. 3 n < θ = (1) The difference The difference yaw[ ] yaw[ ] Measurg Pots -3 (a) n 1 1 Measurg pots -1-4 Figure 13. (a) The difference headg angle before corrected (b) difference headg angle after correction. Measurg Pots Measurg pots The trajectory (a) corrected by turn-straight-state threshold (b) detection method proposed Figure 13. this (a) paper The difference is shown Figure headg 14. It angle can be before seen corrected from figure (b) that difference output Figure 13. (a) The trajectory difference through headg wall was angle reduced, before corrected headg angle after correction. but as time creased (b) difference number headg angle after correction. steps creased, re will still be a trajectory through wall later period, resultg The positiong trajectory failure. corrected by turn-straight-state threshold detection method proposed this paper is shown Figure 14. It can be seen from figure that output trajectory through wall was reduced, but as time creased number steps creased, re will still be a trajectory through wall later period, resultg positiong failure. 2 The difference yaw[ ] The difference yaw[ ] 1-1 ors (b)

16 Micromaches 218, 9, The trajectory corrected by turn-straight-state threshold detection method proposed this paper is shown Figure 14. It can be seen from figure that output trajectory through wall was reduced, but as time creased number steps creased, re will still be a trajectory through wall later period, resultg Micromaches positiong 218, failure. 9, x FOR PEER REVIEW Micromaches 218, 9, x FOR PEER REVIEW Y Y Position/m Figure 14. Correction Correction trajectory trajectory based on based turn-straight-state on turn-straight-state threshold threshold detection Figure 14. Correction trajectory based on turn-straight-state threshold detection method. detection method. method. The detection turng pot was matched with feature pots feature The detection turng pot was matched with feature pots feature pot map, turng pot position was reset to matched feature pot pot map, turng turng pot pot position position was reset was toreset matched to matched feature pot feature position, pot position, weight value was updated at same time, while position drift was modified position, weight value weight wasvalue updated was at updated same at time, same while time, position while drift position was modified drift was to modified reduce to reduce degree divergence particle at corner. The corrected trajectory is to reduce degree divergence degree divergence particle atparticle corner. at The corner. corrected The corrected trajectory is trajectory shown is shown red trajectory Figure 1. The s fal position coordate was shown red trajectory red Figure trajectory 1. The s Figure 1. fal The position s coordate fal was position (.1138,.4618) coordate after was (.1138,.4618) after correction. From figure, it can be seen that PF-SLAM algorithm based correction. (.1138,.4618) From after figure, correction. it can be From seen that figure, PF-SLAM it can be algorithm seen that based PF-SLAM on feature algorithm potbased map on feature pot map proposed this paper effectively suppressed accumulation proposed on feature thispot paper map effectively proposed suppressed this paper accumulation effectively suppressed headg accumulation error particle headg error particle filter over time. Compared with trajectory before filter headg overerror time. Compared particle with filter over time. trajectory Compared beforewith correction, phenomenon trajectory passg before correction, phenomenon passg through wall particles was basically solved, through correction, wall phenomenon particles was passg basicallythrough solved, wall positiong particles accuracy was basically door solved, s positiong accuracy door s was greatly improved. was positiong greatly improved. accuracy door s was greatly improved. 1 1 Y Y Position/m Figure 1. Pedestrian terior trajectory corrected based on this algorithm. Figure 1. Pedestrian terior trajectory corrected based on this algorithm. The turng pots walkg process above experimental s all matched The turng pots walkg process above experimental s all matched stored feature pots feature pot map. In followg experiments, s still stored feature pots feature pot map. In followg experiments, s still started from same startg pot entered two rooms. The experimenter started with (,) started from same startg pot entered two rooms. The experimenter started with (,) eventually returned to startg pot. The experimenter started with (,) stopped at eventually returned to startg pot. The experimenter started with (,) stopped at preset fal position pot (62,). The experimental walkg time is s, total 17 steps, preset fal position pot (62,). The experimental walkg time is s, a total 17 steps, walkg distance is m. Figure 16 shows trajectory particle filter output walkg distance is m. Figure 16 shows trajectory particle filter output

17 Micromaches 218, 9, The turng pots walkg process above experimental s all matched stored feature pots feature pot map. In followg experiments, s still started from same startg pot entered two rooms. The experimenter started with (,) eventually returned to startg pot. The experimenter started with (,) stopped at preset fal position pot (62,). The experimental walkg time is s, a total 17 steps, walkg distance is m. Figure 16 shows trajectory particle filter output before correction. The s fal position coordate was (61.4,1.236) before correction. It can be seen from figure that particles appeared through wall some trajectory positiong Micromaches failed. 218, 9, x FOR PEER REVIEW Y Position/m Figure 16. Pedestrian trajectory before correction. The The PF-SLAM PF-SLAM door door positiong positiong algorithm algorithm based based on on feature feature pot pot map map proposed proposed this this paper paper corrected corrected headg headg corrected corrected position position error through error through feature pot feature matchg. pot matchg. The corrected The corrected trajectory is trajectory shown is Figure shown 17, accordg Figure 17, to accordg Equation () to Equation width () experimental width site corridor, experimental turng site corridor, pot status was turng determed pot as: status was determed as: ( ) n i l n n ( T ). T = 1 d n i T. (16) T = ors (16) ors In Figure 17, trajectory basically solved phenomenon particles through wall, In Figure 17, trajectory basically solved phenomenon particles through wall, positiong accuracy was greatly improved. The s fal position coordate was positiong accuracy was greatly improved. The s fal position coordate was (61.47,.623) after correction. The green circles figure represent turng pots that were (61.47,.623) after correction. The green circles figure represent turng pots that were matched to feature pots. The remag turng pots represent turng pots matched to feature pots. The remag turng pots represent turng pots that were not matched to current feature pot database extracted by turn-straight-state that were not matched to current feature pot database extracted by turn-straight-state threshold detection method. Accordg to stored ma path position data formation feature threshold detection method. Accordg to stored ma path position data formation pot map, only turng pot on ma path was extracted added to feature pot map feature pot map, only turng pot on ma path was extracted added to feature to update map, as shown by blue circle. pot map to update map, as shown by blue circle. In order to furr evaluate accuracy door positiong algorithm proposed In order to furr evaluate accuracy door positiong algorithm this paper, Table 1 shows a comparison parameters before after correction proposed this paper, Table 1 shows a comparison parameters before after correction algorithm. The position error is calculated by dividg distance between preset fal position algorithm. The position error is calculated by dividg distance between preset fal experimental output fal position by percentage total distance. It can be seen from position experimental output fal position by percentage total distance. It can be Table 1 that PF-SLAM door location algorithm based on a feature pot map algorithm seen from Table 1 that PF-SLAM door location algorithm based on a feature pot proposed this paper improves door positiong accuracy greatly reduces map algorithm proposed this paper improves door positiong accuracy number positiong efforts that failed due to particles passg through wall. greatly reduces number positiong efforts that failed due to particles passg through wall.

18 Micromaches 218, 9, Micromaches 218, 9, x FOR PEER REVIEW Y Position/m Figure Figure Pedestrian trajectory after correction. Table 1. Comparison parameters before after correction algorithm. Table 1. Comparison parameters before after correction algorithm. Parameters Experiment 1 Experiment 2 Position Parameters error (%) Experiment Experiment Before Correction Number Position through error wall (%) Before Correction Position error (%) After Correction Number through wall 6 3 Number through wall Position error (%) After Correction Number through wall 6. Conclusions This paper studied door positiong algorithm proposed a PF-SLAM 6. Conclusions door positiong algorithm based on a feature pot map. Without prior knowledge This map, paper studied door door positiong could positiong be achieved algorithm only through proposed ertial a PF-SLAM navigation door device positiong IMU provided algorithm a solution basedfor on a feature situation pot emergency map. Without rescue prior knowledge ability to obta map, door maps a timely positiong manner. could The be PF-SLAM achieved algorithm only through was implemented ertialby navigation buildg a device feature IMU provided pot map a solution door for ma situation paths through emergency extracted rescue feature pots, ability to ma obta path door feature maps pot map can reflect general situation terior buildg. A turn-straight-state a timely manner. The PF-SLAM algorithm was implemented by buildg a feature pot map threshold detection method was proposed to detect straight-walkg phase door ma paths through extracted feature pots, ma path feature pot map can turng pots. In straight-walkg phase, difference headg angle was corrected to reflect suppress general headg situation error accumulatg terior over time. buildg. The detected A turn-straight-state turng pots threshold ma detection path method were was matched proposed with to detect feature pot map straight-walkg weights were updated, phaseeffectively turng suppressg pots. In straight-walkg divergence phase, particles difference at flection headg pot. Experimental angle was corrected results showed to suppress that door headg error accumulatg positiong over time. algorithm The detected proposed turng this pots paper could ma effectively path were suppress matched with feature accumulation pot map headg errors, weights were positiong updated, results effectively were more suppressg accurate. It effectively divergence solves particles problem at flection positiong pot. failures Experimental that rely solely results on showed strapdown that ertial door navigation. positiong algorithm proposed this paper could effectively suppress accumulation headg errors, Author Contributions: J.S. conceived designed method proposed this article, performed positiong analyzed results experiments, were more wrote accurate. revised It effectively manuscript. solves M.R. provided problem algorithmic positiong ideas failures revised that rely solely opions. onp.w. strapdown contributed ertial to layout navigation. sis J.M. collected some data. Acknowledgments: This study was supported by General program science technology Author Contributions: J.S. conceived designed method proposed this article, performed analyzed development project Beijg Municipal Education Commission (Grant No. KM21616). experiments, wrote revised manuscript. M.R. provided algorithmic ideas revised opions. P.W. contributed Conflicts to Interest: layout The authors sis declare no conflicts J.M. collected terest. some data. Acknowledgments: This study was supported by General program science technology development project References Beijg Municipal Education Commission (Grant No. KM21616). Conflicts 1. Akyildiz, Interest: I.F.; The Su, authors W.L.; Sankarasubramaniam, declare no conflictsy.; Cayirci, terest. E. A survey on sensor networks. IEEE Commun. Mag. 22, 4, References 1. Akyildiz, I.F.; Su, W.L.; Sankarasubramaniam, Y.; Cayirci, E. A survey on sensor networks. IEEE Commun. Mag. 22, 4, [CrossRef]

19 Micromaches 218, 9, Hazas, M.; Scott, J.; Krumm, J. Location-aware computg comes age. Computer 24, 37, [CrossRef] 3. Campos, R.S.; Lovisolo, L.; de Campos, M.L.R. Wi-fi multi-floor door positiong considerg architectural aspects controlled computational complexity. Expert Syst. Appl. 214, 41, [CrossRef] 4. Li, N.; Chen, J.B.; Yuan, Y. Indoor tegrated localization strategy based on WiFi/PDR. Zhongguo Guanxg Jishu Xuebao/J. Ch. Inert. Technol. 217, 2, Skog, I.; Nilsson, J.O.; Hel, P. Evaluation zero-velocity detectors for foot-mounted ertial navigation systems. In Proceedgs 21 International Conference on Indoor Positiong Indoor Navigation (IPIN), Zurich, Switzerl, 1 17 September 21; pp Gu, Y.; Song, Q.; Li, Y.H.; Ma, M.; Zhou, Z.M. An anchor-based navigation approach usg only ertial sensors. Sensors 216, 16, 334. [CrossRef] [PubMed] 7. Sh, S.H.; Park, C.G.; Choi, S. New map-matchg algorithm usg virtual track for dead reckong. ETRI J. 21, 32, [CrossRef] 8. Zhou, J.Y.; Shi, J. Rfid localization algorithms applications-a review. J. Intell. Manuf. 29, 2, [CrossRef] 9. Reza, A.W.; Geok, T.K.; Dimyati, K. Trackg via square grid RFID reader positiong diffusion algorithm. Wirel. Pers. Commun. 211, 61, [CrossRef] 1. Kasmi, Z.; Norrde, A.; Blankenbach, J. Towards a decentralized magnetic door positiong system. Sensors 21, 1, [CrossRef] [PubMed] 11. Zhang, Y.; Cao, B. Strapdown algorithm research based on improved equivalent rotation vector. Ship Electron. Eng. 217, 9, Diaz, E.M.; Caamano, M. Lmark-based onle drift compensation algorithm for ertial navigation. In Proceedgs 217 International Conference on Indoor Positiong Indoor Navigation (IPIN), Sapporo, Japan, September Ren, M.R.; Guo, H.Y.; Shi, J.J.; Meng, J. Indoor navigation based on conditional rom field algorithm. Micromaches 217, 8, 32. [CrossRef] 14. Sun, Y.C.; Wang, B. Indoor corner recognition from crowdsourced trajectories usg smartphone sensors. Expert Syst. Appl. 217, 82, [CrossRef] 1. Smith, R.; Self, M.; Cheeseman, P. Estimatg uncerta spatial relationship robotics. In Autonomous Robot Vehicles; Sprge-Verlag: New York, NY, USA, 199; pp Arulampalam, M.S.; Maskell, S.; Gordon, N.; Clapp, T. A tutorial on particle filters for onle nonlear/non-gaussian bayesian trackg. IEEE Trans. Signal Process. 22,, [CrossRef] 17. Li, X.B.; Zhang, C.L.; Yan, L.; Han, S.; Guan, X.P. A support vector learng-based particle filter scheme for target localization communication-constraed underwater acoustic sensor networks. Sensors 218, 18, 8. [CrossRef] [PubMed] 18. Bailey, T.; Nieto, J.; Nebot, E. Consistency fastslam algorithm. In Proceedgs 26 IEEE International Conference on Robotics Automation (ICRA), Orlo, FL, USA, 1 19 May 26; pp Ren, M.R.; Pan, K.; Liu, Y.H.; Guo, H.Y.; Zhang, X.D.; Wang, P. A novel navigation algorithm for a foot-mounted ertial-sensor-based system. Sensors 216, 16, 139. [CrossRef] [PubMed] 2. Lu, Y.; Wei, D.Y.; Lai, Q.F.; Li, W.; Yuan, H. A context-recognition-aided pdr localization method based on hidden markov model. Sensors 216, 16, 23. [CrossRef] [PubMed] 21. Xiong, Y.Q.; Zhang, Y.S.; Wang, Z.Q.; Li, M. Research for navigation positiong method based on mems sensors. In Proceedgs 21 34th Chese Control Conference (CCC), Hangzhou, Cha, 28 3 July 21; pp Doucet, A.; Godsill, S.J.; Andrieu, C. On Sequential Simulation-Based Methods for Bayesian Filterg; Technical Report CUED/F-INFENG/TR31; Department Engeerg, University Cambridge: Cambridge, UK, Beauregard, S.; Widyawan, T.; Klepal, M. Indoor pdr performance enhancement usg mimal map formation particle filters. In Proceedgs 28 IEEE/ION Position, Location Navigation Symposium, Monterey, CA, USA, 8 May 28; pp by authors. Licensee MDPI, Basel, Switzerl. This article is an open access article distributed under terms conditions Creative Commons Attribution (CC BY) license (