System and method for tracking obstacles by an autonomous vehicle. Localization sensors (i.e., sensors to measure pitch, roll, and yaw, and systems including an inertial navigation system, a compass, a global positioning system, or an odometer) detect the position of the vehicle. Perception sensors (e.g., LIDAR, stereo vision, infrared vision, radar, or sonar) assess the environment about the vehicle. Using these sensors, locations of terrain features relative to the vehicle are computed and kept up-to-date. The vehicle trajectory is adjusted to avoid terrain features that are obstacles in the path of the vehicle.