In order to address the low accuracy issue of IMU-based(inertial-measure-unit-based) dead reckoning and the difficulty of path optimization between the indoor floors and other problems, a series of work are carried out in this paper. First of all, a three-dimension path optimization algorithm, A-DP, is proposed based on the heuristic dynamic programming. Then, with the optimal path constraint, the HDE algorithm and the accuracy of the direction of travel(SPM-HDE) are improved. Finally, combining with IMU navigation technologies, the navigation in indoor three-dimensional space is accomplished. Test results show that both the efficiency of the algorithm and the accuracy of the dead reckoning are greatly improved.