The fusion algorithm based on multi-state constrained Kalman filtering solely uses a single frame imagefor pose estimation. If the initialization is incorrect, then it can cause severe divergence in visual pose estimation.Furthermore, each visual feature point in the system state vector can easily lead to computational burden to thesystem. Given the aforementioned problems, an improved keyframe selection algorithm is proposed, which usesmultiple visual keyframes to constrain the same feature points for reducing visual measurement errors and improvingpositioning accuracy. Simultaneously, only the camera pose calculated from keyframes is integrated into the system statevector, which can effectively reduce system computation. The experiment shows that the improved algorithm enhances positioningaccuracy and computational efficiency by 29.09% and 32.2%, respectively, when compared to EKF. Additionally,the proposed algorithm increases computational efficiency by 35.48% when compared to that of Orb-slam2.