Terminal guidance of vehicles is accomplished by accurately determining true position and speed of the vehicle from terrain contour measurements and updating a dead reckoning navigation system with precision to permit navigation by following a programmed flight path. A narrow beam radar altitude sensor measures vehicle clearance altitudes and a reference altitude sensor measures the vehicle clearance above a reference level. These measurements are input to a digital processor for comparison to stored digital terrain contour reference data. Recursive data matching is performed in the digital processor by a nonlinear Kalman filtering technique to provide vehicle position and vehicle velocity correction signals to a navigation controller that generates steering commands to the vehicle control system. Three vehicle position and three vehicle velocity signals are generated and updated in the nonlinear Kalman filtering technique to give near real-time three dimensional position and velocity information.