A system for determining the shape of a surgical instrument includes an elongate instrument body having a selectively steerable distal tip, a pair of automatically controlled segments proximal to the selectively steerable distal tip, and a joint that couples the pair of adjacent automatically controlled segments together. An actuator changes an angle of the joint and a position encoder provides information associated with the joint angle. A controller receives the information associated with the angle of the joint and generates a three dimensional model of a shape of the instrument. A method for determining the three dimensional shape of an instrument includes providing axial position data to a controller, providing angular position data to the controller from a respective segment controller, and generating a three-dimensional model of a shape of said instrument using the axial position data and the angular position data.