Navigating in a non – GPS environment is still a challenging research issue. With the incremental use of GPS blockers, and the increase of fighting in tunnels, the need for inertial navigating systems is rising. In this paper, accelerometers, Gyroscopes and magnetometers based algorithm is approached. A 3-axis acceleration sensor provides signals which are processed and filtered, combining with a 3-axis Gyroscope signals for step identification and measurement. Using phase detection in order to classify horizontal steps and stair climbing. Combining with magnetometer signals, a 3D trail of the pedestrian is portrayed. Using grid search for optimal parameter selection, the algorithm achieves 95% step detection and 90% correct step classification.