The paper proposes an algorithm based on the Multi-State Constraint Kalman Filter (MSCKF) algorithm to construct the map for robots special in the poor GPS signal environment. We can calculate the position of the robots with the data collected by inertial measurement unit and the features extracted by the camera with MSCKF algorithm in a tight couple way. The paper focuses on the way of optimizing the position because we adopt it to compute Kalman gain for updating the state of robots. In order to reduce the processing time, we design a novel fast Gauss–Newton MSCKF algorithm to complete the nonlinear optimization. Compared with the performance of conventional MSCKF algorithm, the novel fast-location algorithm can reduce the processing time with the kitti datasets.