In the past decade, positioning and navigation for indoor and underground scenarios have been a hot research topic. The Global Navigation Satellite System (GNSS) is the primary system for outdoor scenarios. However, there is no dominant technology for indoor and underground scenarios. Visible light positioning shows its potential for indoor and underground positioning due to its important features, such as high bandwidth for high-speed transmission, energy-efficient, long lifetime, and cost-efficiency. This presentation covers our works to improve the accuracy and robustness of the visible light positioning. The first effort is to reduce the effects of the environment light and noise on the visible light positioning system, which has been hardly focused on before. Then, we try different data fusion methods, including non-linear smoother, non-linear optimization, and machine learning algorithms, to improve the performance of the visible light positioning system. The preliminary results show the proposed visible light positioning system can achieve centimeter-level accuracy for indoor and underground scenarios. In the future, we will further evaluate this technology for large indoor and underground experimental areas. Furthermore, We will integrate the visible light positioning with other techonogies, such as inertial navigation and SLAM for higher performance.