The current optimization approaches of construction machinery are mainly based on internal sensors. However, the decision of a reasonable strategy is not only determined by its intrinsic signals, but also very strongly by environmental information, such as terrain. Due to the lack of methods to aware the environment, although many promising methods were proposed to improve the mobile machines' efficiency, most construction machinery engineers must focus on the most hazardous situation and have to give the highest priority to safety when designing. Consequently, the industry does not fully benefited from the existing research. Also, due to the dynamically changing of the construction site and the consequent absence of a high definition map, the localization of mobile machines is still a challenging task. In this study, we proposed a SLAM method to create a multi-layer gird map for the construction site so that the machine can have the environmental information and be optimized accordingly. Concretely, after the machine passes by, we can get the local information and record it. Combining with positioning technology, we then create a map of the interesting places of the construction site. As a result of our research gathered from Gazebo, we showed that a suitable layout is the combination of 1 IMU and 2 differential GPS antennas using the unscented Kalman filter, which keeps the average distance error lower than 2m and the mapping error lower than 2% in the harsh environment. As an outlook, our SLAM technology provides the cornerstone to activate many efficiency improvement approaches.