Li-Ta Hsu

Baoshan Song, Matthew Giamou, Penggao Yan et al.

Estimating the absolute orientation of a local system relative to a global navigation satellite system (GNSS) reference often suffers from local minima and high dependency on satellite availability. Existing methods for this alignment task rely on abundant satellites unavailable in GNSS-degraded environments, or use local optimization methods which cannot guarantee the optimality of a solution. This work introduces a globally optimal solver that transforms raw pseudo-range or Doppler measurements into a convexly relaxed problem. The proposed method is certifiable, meaning it can numerically verify the correctness of the result, filling a gap where existing local optimizers fail. We first formulate the original frame alignment problem as a nonconvex quadratically constrained quadratic program (QCQP) problem and relax the QCQP problem to a concave Lagrangian dual problem that provides a lower cost bound for the original problem. Then we perform relaxation tightness and observability analysis to derive criteria for certifiable optimality of the solution. Finally, simulation and real world experiments are conducted to evaluate the proposed method. The experiments show that our method provides certifiably optimal solutions even with only 2 satellites with Doppler measurements and 2D vehicle motion, while the traditional velocity-based VOBA method and the advanced GVINS alignment technique may fail or converge to local optima without notice. To support the development of GNSS-based navigation techniques in robotics, all code and data are open-sourced at https://github.com/Baoshan-Song/Certifiable-Doppler-alignment.

Baoshan Song, Ruijie Xu, Zhi Zhan et al.

Sliding window factor graph optimization (SW-FGO) is widely recognized for its robustness, yet its theoretical relationship with the extended Kalman filter (EKF) remains a subject of debate. This paper establishes the sufficient conditions to bridge SW-FGO with the iterated extended Kalman filter (IEKF). We introduce recursive FGO (Re-FGO), a conceptual perspective that employs a two-stage marginalization pipeline to mathematically degenerate the factor graph optimization to the IEKF recursive update. By enforcing the Markov assumption and a single-state window, we prove the theoretical equivalence between the IEKF and Re-FGO. This degeneration is validated through simulations and real-world urban GNSS and INS tightly coupled fusion experiments. The results confirm that Re-FGO exactly reproduces IEKF estimation behavior, demonstrating that the two-stage marginalization pipeline is foundational to enforce structural consistency, thereby successfully uniting graph-based smoothing and filtering paradigms under unified optimization principles.

Baoshan Song, Feng Huang, Li-Ta Hsu

Decentralized multi-robot LiDAR-SLAM is essential for collaborative missions but faces significant challenges in maintaining global consistency. Existing frameworks predominantly rely on local-search optimization or one-time coordinate alignment, which are prone to suboptimal convergence and long-term inconsistency, especially in large-scale or degenerate environments. To address these limitations, this paper presents the first decentralized LiDAR-SLAM system that integrates a state-of-the-art certifiably optimal Pose Graph Optimization (PGO) backend. By leveraging the Riemannian Block Coordinate Descent (RBCD) algorithm, our system ensures globally consistent trajectory estimation without requiring accurate initial guesses. Experimental results demonstrate that the proposed framework achieves superior robustness, improving trajectory RMSE by up to 48.9% compared to the state-of-the-art DiSCo-SLAM.

Max J. L. Lee, Ju Lin, Li-Ta Hsu

We propose a feasibility study for real-time automated data standardization leveraging Large Language Models (LLMs) to enhance seamless positioning systems in IoT environments. By integrating and standardizing heterogeneous sensor data from smartphones, IoT devices, and dedicated systems such as Ultra-Wideband (UWB), our study ensures data compatibility and improves positioning accuracy using the Extended Kalman Filter (EKF). The core components include the Intelligent Data Standardization Module (IDSM), which employs a fine-tuned LLM to convert varied sensor data into a standardized format, and the Transformation Rule Generation Module (TRGM), which automates the creation of transformation rules and scripts for ongoing data standardization. Evaluated in real-time environments, our study demonstrates adaptability and scalability, enhancing operational efficiency and accuracy in seamless navigation. This study underscores the potential of advanced LLMs in overcoming sensor data integration complexities, paving the way for more scalable and precise IoT navigation solutions.

Penggao Yan, Ronghe Jin, Junyi Zhang et al.

Legacy and advanced receiver autonomous integrity monitoring (RAIM/ARAIM) rely on Gaussian error models that can be overly conservative for real-world non-Gaussian errors. This paper proposes an extended jackknife detector capable of detecting multiple simultaneous faults with non-Gaussian nominal errors. Furthermore, an integrity monitoring algorithm, jackknife ARAIM, is developed by systematically exploiting the properties of the jackknife detector in the range domain. We prove that the proposed method has equivalent monitoring performance with the solution separation (SS) ARAIM, but is significantly computationally efficient for single-fault cases with non-Gaussian nominal errors, while maintaining similar efficiency to SS ARAIM for multiple-fault cases. The proposed method is examined in worldwide simulations, with the nominal measurement error simulated based on authentic experimental data, which reveals different findings in existing research. In a single Global Positioning System (GPS) constellation setting, the proposed method can reduce the 99.5 percentile vertical protection level (VPL) below 45 m, outperforming 50 m VPL produced by the ARAIM algorithm using Gaussian nominal error models. In GPS-Galileo dual-constellation setting, while these Gaussian-based ARAIM algorithms suffer VPL inflation over 60 m due to Galileo's heavy-tailed errors, the proposed method maintains VPL below 40 m, achieving over 92% normal operations for 35 m Vertical Alert Limit. Moreover, we tentatively implement the SS ARAIM using non-Gaussian overbounds and compare it with the proposed Jackknife ARAIM method regarding computation efficiency. The proposed method achieves up to 59.4% reduction in median processing time compared to SS ARAIM in single-constellation scenarios.

Liang Qian, Penggao Yan, Penghui Xu et al.

Global navigation satellite system (GNSS) positioning is widely used for urban navigation, but the covariance reported by the GNSS solver is often unreliable in urban canyons. Existing differentiable factor graph optimization (DFGO) methods already learn measurement weighting through the solver, but they still use position-only objectives. As a result, the mean estimate may improve while the reported covariance remains too small, too large, or wrong in shape. In this work, we propose CredibleDFGO (CDFGO), a differentiable GNSS factor graph framework that makes covariance credibility an explicit training target. The Weighting Generation Network (WGN) predicts per-satellite reliability weights. The differentiable Gauss--Newton solver maps these weights to a position estimate and posterior covariance, and proper scoring rules supervise the East--North predictive distribution end-to-end. We study negative log-likelihood (NLL), Energy Score (ES), and their combination. Results on three UrbanNav test scenes show consistent gains in uncertainty credibility. Positioning accuracy also improves on the medium-urban and harsh-urban scenes, and the mean horizontal error and 95th-percentile error improve on the deep-urban scene. On the harsh-urban Mong Kok (MK) scene, CDFGO-Combined reduces the mean horizontal error from 13.77\,m to 11.68\,m, reduces NLL from 40.63 to 6.59, and reduces ES from 12.31 to 9.05. The case studies link the MK improvement to better axis-wise consistency, more credible local covariance ellipses, and satellite-level reweighting.

Penggao Yan, Xingqun Zhan, Rui Sun et al.

State estimators often provide self-assessed uncertainty metrics, such as covariance matrices, whose credibility is critical for downstream tasks. However, these self-assessments can be misleading due to underlying modeling violations like noise model mismatch (NMM) or system model misspecification (SMM). This letter addresses this problem by developing a unified, multi-metric framework that integrates noncredibility index (NCI), negative log-likelihood (NLL), and energy score (ES) metrics, featuring an empirical location test (ELT) to detect system model bias and a directional probing technique that uses the metrics' asymmetric sensitivities to distinguish NMM from SMM. Monte Carlo simulations reveal that the proposed method achieves excellent diagnosis accuracy (80-100%) and significantly outperforms single-metric diagnosis methods. The effectiveness of the proposed method is further validated on a real-world UWB positioning dataset. This framework provides a practical tool for turning patterns of credibility indicators into actionable diagnoses of model deficiencies.

Weisong Wen, Li-Ta Hsu

In this paper, we propose a 3D LiDAR aided global navigation satellite system (GNSS) non-line-of-sight (NLOS) mitigation method caused by both static buildings and dynamic objects. A sliding window map describing the surrounding of the ego-vehicle is first generated, based on real-time 3D point clouds from a 3D LiDAR sensor. Then, NLOS receptions are detected based on the sliding window map using a proposed fast searching method which is free of the initial guess of the position of the GNSS receiver. Instead of directly excluding the detected NLOS satellites from further positioning estimation, this paper rectifies the pseudorange measurement model by (1) correcting the pseudorange measurements if the reflecting point of NLOS signals is detected inside the sliding window map, and (2) remodeling the uncertainty of the NLOS pseudorange measurement using a novel weighting scheme. We evaluated the performance of the proposed method in several typical urban canyons in Hong Kong using an automobile-level GNSS receiver. Moreover, we also evaluate the potential of the proposed NLOS mitigation method in GNSS and inertial navigation systems integration via factor graph optimization.

Xiwei Bai, Weisong Wen, Li-Ta Hsu

This paper proposes an improved global navigation satellite system (GNSS) positioning method that explores the time correlation between consecutive epochs of the code and carrier phase measurements which significantly increases the robustness against outlier measurements. Instead of relying on the time difference carrier phase (TDCP) which only considers two neighboring epochs using an extended Kalman filter (EKF) estimator, this paper proposed to employ the carrier-phase measurements inside a window, the so-called window carrier-phase (WCP), to constrain the states inside a factor graph. A left null space matrix is employed to eliminate the shared unknown ambiguity variables and therefore, correlated the associated states inside the WCP. Then the pseudorange, Doppler, and the constructed WCP measurements are integrated simultaneously using factor graph optimization (FGO) to estimate the state of the GNSS receiver. We evaluated the performance of the proposed method in two typical urban canyons in Hong Kong, achieving the mean positioning error of 1.76 meters and 2.96 meters, respectively, using the automobile-level GNSS receiver. Meanwhile, the effectiveness of the proposed method is further evaluated using a low-cost smartphone level GNSS receiver and similar improvement is also obtained, compared with several existing GNSS positioning methods.

Weisong Wen, Guohao Zhang, Li-Ta Hsu

Accurate and globally referenced global navigation satellite system (GNSS) based vehicular positioning can be achieved in outlier-free open areas. However, the performance of GNSS can be significantly degraded by outlier measurements, such as multipath effects and non-line-of-sight (NLOS) receptions arising from signal reflections of buildings. Inspired by the advantage of batch historical data in resisting outlier measurements, in this paper, we propose a graduated non-convexity factor graph optimization (FGO-GNC) to improve the GNSS positioning performance, where the impact of GNSS outliers is mitigated by estimating the optimal weightings of GNSS measurements. Different from the existing local solutions, the proposed FGO-GNC employs the non-convex Geman McClure (GM) function to globally estimate the weightings of GNSS measurements via a coarse-to-fine relaxation. The effectiveness of the proposed method is verified through several challenging datasets collected in urban canyons of Hong Kong using automobile level and low-cost smartphone level GNSS receivers.

Weisong Wen, Li-Ta Hsu

Global navigation satellite systems (GNSS) are one of the utterly popular sources for providing globally referenced positioning for autonomous systems. However, the performance of the GNSS positioning is significantly challenged in urban canyons, due to the signal reflection and blockage from buildings. Given the fact that the GNSS measurements are highly environmentally dependent and time-correlated, the conventional filtering-based method for GNSS positioning cannot simultaneously explore the time-correlation among historical measurements. As a result, the filtering-based estimator is sensitive to unexpected outlier measurements. In this paper, we present a factor graph-based formulation for GNSS positioning and real-time kinematic (RTK). The formulated factor graph framework effectively explores the time-correlation of pseudorange, carrier-phase, and doppler measurements, and leads to the non-minimal state estimation of the GNSS receiver. The feasibility of the proposed method is evaluated using datasets collected in challenging urban canyons of Hong Kong and significantly improved positioning accuracy is obtained, compared with the filtering-based estimator.

Feng Huang, Weisong Wen, Jiachen Zhang et al.

Robust and precise localization is essential for the autonomous system with navigation requirements. Light detection and ranging (LiDAR) odometry is extensively studied in the past decades to achieve this goal. Satisfactory accuracy can be achieved in scenarios with abundant environmental features using existing LiDAR odometry (LO) algorithms. Unfortunately, the performance of the LiDAR odometry is significantly degraded in urban canyons with numerous dynamic objects and complex environmental structures. Meanwhile, it is still not clear from the existing literature which LO algorithms perform well in such challenging environments. To fill this gap, this paper evaluates an array of popular and extensively studied LO pipelines using the datasets collected in urban canyons of Hong Kong. We present the results in terms of their positioning accuracy and computational efficiency. Three major factors dominating the performance of LO in urban canyons are concluded, including the ego-vehicle dynamic, moving objects, and degree of urbanization. According to our experiment results, point-wise achieves better accuracy in urban canyons while feature-wise achieves cost-efficiency and satisfactory positioning accuracy.

Max Jwo Lem Lee, Li-Ta Hsu

Accurate localization and 3D maps are increasingly needed for various artificial intelligence based IoT applications such as augmented reality, intelligent transportation, crowd monitoring, robotics, etc. This article proposes a novel semantic 3D map change detection and update based on a smartphone visual positioning system (VPS) for the outdoor and indoor environments. The proposed method presents an alternate solution to SLAM for map update in terms of efficiency, cost, availability, and map reuse. Building on existing 3D maps of recent years, a system is designed to use artificial intelligence to identify high-level semantics in images for positioning and map change detection. Then, a virtual LIDAR that estimates the depth of objects in the 3D map is used to generate a compact point cloud to update changes in the scene. We present an excellent performance of localization with respect to other state-of-the-art smartphone positioning solutions to accurately update semantic 3D maps. It is shown that the proposed solution can position users within 1.9m, and update objects with an average error of 2.1m.

Max Jwo Lem Lee, Li-Ta Hsu, Hoi-Fung Ng et al.

Accurate smartphone-based outdoor localization system in deep urban canyons are increasingly needed for various IoT applications such as augmented reality, intelligent transportation, etc. The recently developed feature-based visual positioning system (VPS) by Google detects edges from smartphone images to match with pre-surveyed edges in their map database. As smart cities develop, the building information modeling (BIM) becomes widely available, which provides an opportunity for a new semantic-based VPS. This article proposes a novel 3D city model and semantic-based VPS for accurate and robust pose estimation in urban canyons where global navigation satellite system (GNSS) tends to fail. In the offline stage, a material segmented city model is used to generate segmented images. In the online stage, an image is taken with a smartphone camera that provides textual information about the surrounding environment. The approach utilizes computer vision algorithms to rectify and hand segment between the different types of material identified in the smartphone image. A semantic-based VPS method is then proposed to match the segmented generated images with the segmented smartphone image. Each generated image holds a pose that contains the latitude, longitude, altitude, yaw, pitch, and roll. The candidate with the maximum likelihood is regarded as the precise pose of the user. The positioning results achieves 2.0m level accuracy in common high rise along street, 5.5m in foliage dense environment and 15.7m in alleyway. A 45% positioning improvement to current state-of-the-art method. The estimation of yaw achieves 2.3° level accuracy, 8 times the improvement to smartphone IMU.

Jiang Yue, Weisong Wen, Jing Han et al.

LiDAR-based SLAM algorithms are extensively studied to providing robust and accurate positioning for autonomous driving vehicles (ADV) in the past decades. Satisfactory performance can be obtained using high-grade 3D LiDAR with 64 channels, which can provide dense point clouds. Unfortunately, the high price significantly prevents its extensive commercialization in ADV. The cost-effective 3D LiDAR with 16 channels is a promising replacement. However, only limited and sparse point clouds can be provided by the 16 channels LiDAR, which cannot guarantee sufficient positioning accuracy for ADV in challenging dynamic environments. The high-resolution image from the low-cost camera can provide ample information about the surroundings. However, the explicit depth information is not available from the image. Inspired by the complementariness of 3D LiDAR and camera, this paper proposes to make use of the high-resolution images from a camera to enrich the raw 3D point clouds from the low-cost 16 channels LiDAR based on a state-of-the-art deep learning algorithm. An ERFNet is firstly employed to segment the image with the aid of the raw sparse 3D point clouds. Meanwhile, the sparse convolutional neural network is employed to predict the dense point clouds based on raw sparse 3D point clouds. Then, the predicted dense point clouds are fused with the segmentation outputs from ERFnet using a novel multi-layer convolutional neural network to refine the predicted 3D point clouds. Finally, the enriched point clouds are employed to perform LiDAR SLAM based on the state-of-the-art normal distribution transform (NDT). We tested our approach on the re-edited KITTI datasets: (1)the sparse 3D point clouds are significantly enriched with a mean square error of 1.1m MSE. (2)the map generated from the LiDAR SLAM is denser which includes more details without significant accuracy loss.

Weisong Wen, Tim Pfeifer, Xiwei Bai et al.

The recently proposed factor graph optimization (FGO) is adopted to integrate GNSS/INS attracted lots of attention and improved the performance over the existing EKF-based GNSS/INS integrations. However, a comprehensive comparison of those two GNSS/INS integration schemes in the urban canyon is not available. Moreover, the performance of the FGO-based GNSS/INS integration rely heavily on the size of the window of optimization. Effectively tuning the window size is still an open question. To fill this gap, this paper evaluates both loosely and tightly-coupled integrations using both EKF and FGO via the challenging dataset collected in the urban canyon. The detailed analysis of the results for the advantages of the FGO is also given in this paper by degenerating the FGO-based estimator to an EKF like estimator. More importantly, we analyze the effects of window size against the performance of FGO, by considering both the GNSS pseudorange error distribution and environmental conditions.

Weisong Wen, Yiyang Zhou, Guohao Zhang et al.

Mapping and localization is a critical module of autonomous driving, and significant achievements have been reached in this field. Beyond Global Navigation Satellite System (GNSS), research in point cloud registration, visual feature matching, and inertia navigation has greatly enhanced the accuracy and robustness of mapping and localization in different scenarios. However, highly urbanized scenes are still challenging: LIDAR- and camera-based methods perform poorly with numerous dynamic objects; the GNSS-based solutions experience signal loss and multipath problems; the inertia measurement units (IMU) suffer from drifting. Unfortunately, current public datasets either do not adequately address this urban challenge or do not provide enough sensor information related to mapping and localization. Here we present UrbanLoco: a mapping/localization dataset collected in highly-urbanized environments with a full sensor-suite. The dataset includes 13 trajectories collected in San Francisco and Hong Kong, covering a total length of over 40 kilometers. Our dataset includes a wide variety of urban terrains: urban canyons, bridges, tunnels, sharp turns, etc. More importantly, our dataset includes information from LIDAR, cameras, IMU, and GNSS receivers. Now the dataset is publicly available through the link in the footnote. Dataset Link: https://advdataset2019.wixsite.com/urbanloco.

Weisong Wen, Li-Ta Hsu, Guohao Zhang

Robust and lane-level positioning is essential for autonomous vehicles. As an irreplaceable sensor, LiDAR can provide continuous and high-frequency pose estimation by means of mapping, on condition that enough environment features are available. The error of mapping can accumulate over time. Therefore, LiDAR is usually integrated with other sensors. In diverse urban scenarios, the environment feature availability relies heavily on the traffic (moving and static objects) and the degree of urbanization. Common LiDAR-based SLAM demonstrations tend to be studied in light traffic and less urbanized area. However, its performance can be severely challenged in deep urbanized cities, such as Hong Kong, Tokyo, and New York with dense traffic and tall buildings. This paper proposes to analyze the performance of standalone NDT-based graph SLAM and its reliability estimation in diverse urban scenarios to further evaluate the relationship between the performance of LiDAR-based SLAM and scenario conditions. The normal distribution transform (NDT) is employed to calculate the transformation between frames of point clouds. Then, the LiDAR odometry is performed based on the calculated continuous transformation. The state-of-the-art graph-based optimization is used to integrate the LiDAR odometry measurements to implement optimization. The 3D building models are generated and the definition of the degree of urbanization based on Skyplot is proposed. Experiments are implemented in different scenarios with different degrees of urbanization and traffic conditions. The results show that the performance of the LiDAR-based SLAM using NDT is strongly related to the traffic condition and degree of urbanization.

Weisong Wen, Guohao Zhang, Li-Ta Hsu

Absolute positioning is an essential factor for the arrival of autonomous driving. Global Navigation Satellites System (GNSS) receiver provides absolute localization for it. GNSS solution can provide satisfactory positioning in open or sub-urban areas, however, its performance suffered in super-urbanized area due to the phenomenon which are well-known as multipath effects and NLOS receptions. The effects dominate GNSS positioning performance in the area. The recent proposed 3D map aided (3DMA) GNSS can mitigate most of the multipath effects and NLOS receptions caused by buildings based on 3D city models. However, the same phenomenon caused by moving objects in urban area is currently not modelled in the 3D geographic information system (GIS). Moving objects with tall height, such as the double-decker bus, can also cause NLOS receptions because of the blockage of GNSS signals by surface of objects. Therefore, we present a novel method to exclude the NLOS receptions caused by double-decker bus in highly urbanized area, Hong Kong. To estimate the geometry dimension and orientation relative to GPS receiver, a Euclidean cluster algorithm and a classification method are used to detect the double-decker buses and calculate their relative locations. To increase the accuracy and reliability of the proposed NLOS exclusion method, an NLOS exclusion criterion is proposed to exclude the blocked satellites considering the elevation, signal noise ratio (SNR) and horizontal dilution of precision (HDOP). Finally, GNSS positioning is estimated by weighted least square (WLS) method using the remaining satellites after the NLOS exclusion. A static experiment was performed near a double-decker bus stop in Hong Kong, which verified the effectiveness of the proposed method.