The present invention relates, generally, to controlling a steerable instrument having an elongate body. More particularly, the present invention relates to a system and method for sensing the shape of a steerable instrument and controlling the steerable instrument in response to a control signal from a user input device and a shape signal corresponding to the sensed shape of at least a portion of the steerable instrument. The present invention also relates to a system for sensing the shape of a flexible instrument with an optical shape sensor.