We propose a compact self-contained navigation system with Micro-Electro-Mechanical System (MEMS) inertial sensor and optical navigation sensor for 3-D pipeline mapping. Self-contained navigation system have advantages of robust against severe environmental conditions and also wide applications without external assist such as Global Positioning System (GPS) navigation or localization system based on a map. The goal of this study is to overcome the performance limitations of small, low-grade sensors by combining various sensors with complementary functions and, therefore, to achieve robust tracking performance against severe environmental conditions. The multi-rate EKF solves the frequent outage problem of the optical navigation sensors and the bias drift problem of the MEMS accelerometers. The vector matching algorithm with the gravity field vector solves the bias drift problem of the MEMS gyro except for the yaw in the reference axis. The geometry compensation algorithm minimizes position errors by combining the forward and backward estimation results geometrically. Experiments to verify performance are conducted by driving Radio-Controlled (RC) car equipped with the proposed navigation system on 3-D asphalt pavement. Experimental results show that the proposed navigation system has good performance and estimated position errors are less than one percent, in the range of 855 m. The proposed navigation system can contribute a compact size and robustness not only to 3-D pipeline mapping but also to small mobile robots.