Pedestrian　navigation　system　based　on　inertial　sensors　with　low　cost　and　small　size,　can　realize　independent　positioning　and　do　not　need　to　install　infrastructure　in　advance.　Therefore,　it　receives　extensive　concerns　both　at　home　and　abroad.　While　there　has　been　little　discussion　about　height　error　correction.　When　searching　and　rescuing　in　large　buildings,　vertical　accuracy　may　seem　to　be　more　important　than　horizontal　accuracy.　In　this　paper,　we　study　the　detection　of　stance　phase　and　present　a　floor　matching　algorithm　to　realize　the　height　error　correction.　Firstly,　we　will　distinguish　floor　phase　from　stair　phase.　A　floor　altitude　database　is　built　using　prior　knowledge　and　it　can　determine　which　floor　the　pedestrian　is　walking　on　for　each　given　horizontal　walking　motion　by　matching　altitude　of　each　horizontal　walking　and　the　floor　altitudes　contained　in　the　database.　Finally,　correct　height　error　during　the　stance　phase　intervals　when　pedestrian　is　doing　a　horizontal　walking　motion.　The　experimental　results　show　that　the　method　can　estimate　which　floor　the　pedestrian　locates　on　and　provide　accurate　height　information,　which　can　be　applied　in　the　actual　rescue.