arXiv Science⌕ Search

arXiv subjects

Weisong Wen

Publications and source records attributed to Weisong Wen.

At least 19 recordsLinked to original sources

Two stage GNSS outlier detection for factor graph optimization based GNSS-RTK/INS/odometer fusion

GNSS pseudorange outliers severely disturb carrier-phase ambiguity resolution and positioning, particularly during signal blockages and recovery. Temporal Doppler consistency provides a reliable short-term range-change reference, but requires valid measurements at both endpoints and becomes unavailable upon signal re-acquisition. This paper presents a two-stage pseudorange screening framework that complements Doppler consistency with joint IMU-odometer motion prediction. The second stage compares predicted and measured temporal changes of double-differenced pseudoranges against an accepted anchor epoch without using current GNSS observations in the motion reference. A first-order error model separates anchor, relative-motion, and measurement uncertainties to propagate factor-graph covariance into the chi-square detection gate, and both stages adjust code weights through a closed-form minimum-inflation rule. Under continuous tracking, Doppler screening reduces RTK 3D RMSE by 96.18% and outperforms the a-contrario baseline by 58.56%. When Doppler checks are unavailable, joint IMU-odometer prediction detects a 10-m outlier change in 99.58% of trials after 30 s and 73.72% after 60 s, reducing RIO 3D RMSE by up to 78.51% after outages while RI screening outperforms an inertial-aided baseline by 70.42% in simulation and 39.11% in deep urban canyons.

cs.RO↗

Incremental-Smoothing-Based ARAIM Protection Levels for Tightly Coupled GNSS/IMU Integration in Urban Canyons

Protection levels (PLs) bound position errors under specified fault hypotheses and risk budgets, but advanced receiver autonomous integrity monitoring (ARAIM) is formulated for snapshot GNSS estimation rather than the correlated posterior of tightly coupled GNSS/IMU factor graph optimization (FGO). This paper formulates ARAIM solution separation and PL computation on the FGO posterior. Because current-epoch pseudorange factors enter only the current-state block of the fixed-lag information matrix, the fault-excluded position covariance, solution separation, and nominal-bias propagation follow exactly from the linearized current-state marginal, which re-tains correlations with velocity, IMU-bias, and clock states and with historical information. Woodbury updates with selected-output evaluation compute these quantities for fault-free, single-satellite, constellation-wide, and second-order hypotheses with-out repeated full-state inversion or graph reoptimization. Surrounding monitoring layers screen measurements, handle post-fit outliers, and determine PL validity and availability. In two road experiments, both nominal sequences achieve 100% horizontal and vertical empirical bounding over PL-valid epochs, with a maximum error-to-PL ratio of 0.96. Normalized errors show that the posterior covariance alone does not capture slowly varying ranging biases: a covariance-only bound computed from the same posteriors retains no resolvable margin, and strong temporal correlation limits the empirical evidence to consistency checks rather than validation of the integrity budget. On identical Experiment A posteriors, the proposed method reduces fault-hypothesis evaluation and PL solving time by 65.2% relative to direct full-state inversion with matching integrity decisions, and controlled fault injections demonstrate fail-safe PL invalidation while navigation continues.

cs.RO↗

Initialization-Free LEO Doppler Positioning via Semidefinite Relaxation: A Cold-Start Approach

Low Earth Orbit (LEO) satellite Doppler measurements offer an alternative source of positioning information for Internet of Things devices when Global Navigation Satellite System signals are unavailable. Conventional local optimisation methods depend on an accurate initial position and can fail during a complete cold start. We propose an initialization-free LEO Doppler positioning framework based on semidefinite relaxation. The method reformulates the nonlinear positioning problem into a semidefinite programme to obtain a receiver position estimate without prior location knowledge. We analyse relaxation tightness under noiseless conditions and derive sufficient noise bounds under parameter disturbances. Simulations and field experiments using opportunistic Iridium-NEXT signals evaluate initialization sensitivity, positioning accuracy and computational cost. In the field experiment, the proposed method achieves a three-dimensional positioning error of 140 m without an initial position, whereas local solvers converge to poor solutions under large initial offsets. Using the semidefinite estimate to initialise Gauss-Newton or Dog-Leg reduces the positioning error to 130 m. The framework provides a cold-start initialization stage for subsequent local refinement in opportunistic LEO positioning. The implementation and datasets are publicly available to support reproducibility and further research.

cs.RO↗

GLIO2: A GPU-Parallelized Tightly-Coupled LiDAR-Inertial-GNSS System for Robust and Real-Time Global Localization and Mapping

Globally consistent, real-time state estimation in large-scale, perceptually degraded environments is essential for autonomous vehicles and aerial robots, and requires fusing LiDAR, inertial, and GNSS measurements. Existing fusion methods, however, share a scan-to-map front-end with two failure modes. First, each scan is aligned to an incrementally built map that drifts under degeneracy, and once the estimate diverges the error is irrecoverable. Second, even without divergence, a registration biased by dynamic objects or wrong correspondences is propagated as a single pose constraint with an over-confident covariance, leaving its correspondences unavailable for GNSS to re-weight or relinearize. We propose GLIO2, a tightly-coupled LiDAR-Inertial-GNSS system whose GPU-parallel front-end jointly optimizes scan-to-multiscan LiDAR, IMU pre-integration, and raw GNSS measurements in a single sliding-window factor graph, sustaining real-time operation on edge hardware. A complementary offline back-end reuses the same cached factors to refine the entire trajectory in batch, completing the 30-min, 4.51-km UrbanNav Whampoa sequence in about 24 s. Across three public benchmarks (UrbanNav, MARS-LVIG, M3DGR) and self-collected UAV and vehicle data, GLIO2 attains the best overall accuracy among evaluated systems. On a 5.66-km bridge traversed at up to 96 km/h, where every competing baseline diverges under LiDAR degeneracy, it maintains 1.6 m horizontal accuracy. On an NVIDIA Jetson Orin NX, the full pipeline runs at about 25 Hz (39.60 ms per scan). The source code and datasets will be released.

cs.RO↗

Safer Trajectory Planning with CBF-guided Diffusion Model for Unmanned Aerial Vehicles

Safe and agile trajectory planning is essential for autonomous systems, especially during complex aerobatic maneuvers. Motivated by the recent success of diffusion models in generative tasks, this paper introduces AeroTrajGen, a novel framework for diffusion-based trajectory generation that incorporates control barrier function (CBF)-guided sampling during inference, specifically designed for unmanned aerial vehicles (UAVs). The proposed CBF-guided sampling addresses two critical challenges: (1) mitigating the inherent unpredictability and potential safety violations of diffusion models, and (2) reducing reliance on extensively safety-verified training data. During the reverse diffusion process, CBF-based guidance ensures collision-free trajectories by seamlessly integrating safety constraint gradients with the diffusion model's score function. The model features an obstacle-aware diffusion transformer architecture with multi-modal conditioning, including trajectory history, obstacles, maneuver styles, and goal, enabling the generation of smooth, highly agile trajectories across 14 distinct aerobatic maneuvers. Trained on a dataset of 2,000 expert demonstrations, AeroTrajGen is rigorously evaluated in simulation under multi-obstacle environments. Simulation results demonstrate that CBF-guided sampling reduces collision rates by 94.7% compared to unguided diffusion baselines, while preserving trajectory agility and diversity. Our code is open-sourced at https://github.com/RoboticsPolyu/CBF-DMP.

cs.RO↗

Cauchy-Gaussian Overbound for Heavy-tailed GNSS Measurement Errors

Overbounds of heavy-tailed measurement errors are essential to meet stringent navigation requirements in integrity monitoring applications. This paper proposes to leverage the bounding sharpness of the Cauchy distribution in the core and the Overbounds of heavy-tailed measurement errors are essential for meeting stringent navigation requirements in integrity-monitoring applications. This paper proposes to leverage the bounding sharpness of the Cauchy distribution in the core and the Gaussian distribution in the tails to tightly bound heavy-tailedglobal navigation satellite system measurement errors. We develop a procedure to determine the overbounding parameters for both symmetric unimodal (SU)and non-symmetric unimodal (NSU) heavy-tailed errors and prove that the over-bounding property is preserved through convolution. Experiment results on both simulated and real-world data sets reveal that our method can sharply boundheavy-tailed errors in both the core and tail regions. In the position domain, the proposed method reduces the average vertical protection level by 15% for SU heavy-tailed errors compared with the single-cumulative-density-function Gaussian overbound and by 21%-47% for NSU heavy-tailed errors compared with the navigation discrete envelope and two-step Gaussian overbounds.

stat.AP↗

An Efficient Detector for Faulty GNSS Measurements Detection With Non-Gaussian Noises

Fault detection is crucial to ensure the reliability of navigation systems. However, mainstream fault detection methods are developed based on Gaussian assumptions on nominal errors, while current attempts at non-Gaussian fault detection are either heuristic or lack rigorous statistical properties. The performance and reliability of these methods are challenged in real-world applications. This paper proposes the jackknife detector, a fault detection method tailored for linearized pseudorange-based positioning systems under non-Gaussian nominal errors. Specifically, by leveraging the jackknife technique, a test statistic is derived as a linear combination of measurement errors, eliminating the need for restrictive distributional assumptions while maintaining computational efficiency. A hypothesis test with the Bonferroni correction is then constructed to detect potential faults in measurements. Theoretical analysis proves the equivalence between the jackknife detector and the solution separation (SS) detector, while revealing the former's superior computational efficiency. Through a worldwide simulation and a real-world satellite clock anomaly detection experiment--both involving non-Gaussian nominal errors--the proposed jackknife detector demonstrates equivalent detection performance to the SS detector but achieves a fourfold improvement in computational efficiency. These results highlight the jackknife detector's substantial potential for real-time applications requiring robust and efficient fault detection in non-Gaussian noise environments.

eess.SP↗

UrbanV2X: A Multisensory Vehicle-Infrastructure Dataset for Cooperative Navigation in Urban Areas

Due to the limitations of a single autonomous vehicle, Cellular Vehicle-to-Everything (C-V2X) technology opens a new window for achieving fully autonomous driving through sensor information sharing. However, real-world datasets supporting vehicle-infrastructure cooperative navigation in complex urban environments remain rare. To address this gap, we present UrbanV2X, a comprehensive multisensory dataset collected from vehicles and roadside infrastructure in the Hong Kong C-V2X testbed, designed to support research on smart mobility applications in dense urban areas. Our onboard platform provides synchronized data from multiple industrial cameras, LiDARs, 4D radar, ultra-wideband (UWB), IMU, and high-precision GNSS-RTK/INS navigation systems. Meanwhile, our roadside infrastructure provides LiDAR, GNSS, and UWB measurements. The entire vehicle-infrastructure platform is synchronized using the Precision Time Protocol (PTP), with sensor calibration data provided. We also benchmark various navigation algorithms to evaluate the collected cooperative data. The dataset is publicly available at https://polyu-taslab.github.io/UrbanV2X/.

cs.RO↗

Online IMU-odometer Calibration using GNSS Measurements for Autonomous Ground Vehicle Localization

Accurate calibration of intrinsic (odometer scaling factors) and extrinsic parameters (IMU-odometer translation and rotation) is essential for autonomous ground vehicle localization. Existing GNSS-aided approaches often rely on positioning results or raw measurements without ambiguity resolution, and their observability properties remain underexplored. This paper proposes a tightly coupled online calibration method that fuses IMU, odometer, and raw GNSS measurements (pseudo-range, carrier-phase, and Doppler) within an extendable factor graph optimization (FGO) framework, incorporating outlier mitigation and ambiguity resolution. Observability analysis reveals that two horizontal translation and three rotation parameters are observable under general motion, while vertical translation remains unobservable. Simulation and real-world experiments demonstrate superior calibration and localization performance over state-of-the-art loosely coupled methods. Specifically, the IMU-odometer positioning using our calibrated parameters achieves the absolute maximum error of 17.75 m while the one of LC method is 61.51 m, achieving up to 71.14 percent improvement. To foster further research, we also release the first open-source dataset that combines IMU, 2D odometer, and raw GNSS measurements from both rover and base stations.

cs.RO↗

Integrated Planning and Control on Manifolds: Factor Graph Representation and Toolkit

Model predictive control (MPC) faces significant limitations when applied to systems evolving on nonlinear manifolds, such as robotic attitude dynamics and constrained motion planning, where traditional Euclidean formulations struggle with singularities, over-parameterization, and poor convergence. To overcome these challenges, this paper introduces FactorMPC, a factor-graph based MPC toolkit that unifies system dynamics, constraints, and objectives into a modular, user-friendly, and efficient optimization structure. Our approach natively supports manifold-valued states with Gaussian uncertainties modeled in tangent spaces. By exploiting the sparsity and probabilistic structure of factor graphs, the toolkit achieves real-time performance even for high-dimensional systems with complex constraints. The velocity-extended on-manifold control barrier function (CBF)-based obstacle avoidance factors are designed for safety-critical applications. By bridging graphical models with safety-critical MPC, our work offers a scalable and geometrically consistent framework for integrated planning and control. The simulations and experimental results on the quadrotor demonstrate superior trajectory tracking and obstacle avoidance performance compared to baseline methods. To foster research reproducibility, we have provided open-source implementation offering plug-and-play factors.

cs.RO↗

Wall Inspector: Quadrotor Control in Wall-proximity Through Model Compensation

The safe operation of quadrotors in near-wall urban or indoor environments (e.g., inspection and search-and-rescue missions) is challenged by unmodeled aerodynamic effects arising from wall-proximity. It generates complex vortices that induce destabilizing suction forces, potentially leading to hazardous vibrations or collisions. This paper presents a comprehensive solution featuring (1) a physics-based suction force model that explicitly characterizes the dependency on both rotor speed and wall distance, and (2) a suction-compensated model predictive control (SC-MPC) framework designed to ensure accurate and stable trajectory tracking during wall-proximity operations. The proposed SC-MPC framework incorporates an enhanced dynamics model that accounts for suction force effects, formulated as a factor graph optimization problem integrating system dynamics constraints, trajectory tracking objectives, control input smoothness requirements, and actuator physical limitations. The suction force model parameters are systematically identified through extensive experimental measurements across varying operational conditions. Experimental validation demonstrates SC-MPC's superior performance, achieving 2.1 cm root mean squared error (RMSE) in X-axis and 2.0 cm RMSE in Y-axis position control - representing 74% and 79% improvements over cascaded proportional-integral-derivative (PID) control, and 60% and 53% improvements over standard MPC respectively. The corresponding mean absolute error (MAE) metrics (1.2 cm X-axis, 1.4 cm Y-axis) similarly outperform both baselines. The evaluation platform employs a ducted quadrotor design that provides collision protection while maintaining aerodynamic efficiency. To facilitate reproducibility and community adoption, we have open-sourced our complete implementation, available at https://anonymous.4open.science/r/SC-MPC-6A61.

cs.RO↗

Tightly Joined Positioning and Control Model for Unmanned Aerial Vehicles Based on Factor Graph Optimization

The execution of flight missions by unmanned aerial vehicles (UAV) primarily relies on navigation. In particular, the navigation pipeline has traditionally been divided into positioning and control, operating in a sequential loop. However, the existing navigation pipeline, where the positioning and control are decoupled, struggles to adapt to ubiquitous uncertainties arising from measurement noise, abrupt disturbances, and nonlinear dynamics. As a result, the navigation reliability of the UAV is significantly challenged in complex dynamic areas. For example, the ubiquitous global navigation satellite system (GNSS) positioning can be degraded by the signal reflections from surrounding high-rising buildings in complex urban areas, leading to significantly increased positioning uncertainty. An additional challenge is introduced to the control algorithm due to the complex wind disturbances in urban canyons. Given the fact that the system positioning and control are highly correlated with each other, this research proposes a **tightly joined positioning and control model (JPCM) based on factor graph optimization (FGO)**. In particular, the proposed JPCM combines sensor measurements from positioning and control constraints into a unified probabilistic factor graph. Specifically, the positioning measurements are formulated as the factors in the factor graph. In addition, the model predictive control (MPC) is also formulated as the additional factors in the factor graph. By solving the factor graph contributed by both the positioning-related factors and the MPC-based factors, the complementariness of positioning and control can be deeply exploited. Finally, we validate the effectiveness and resilience of the proposed method using a simulated quadrotor system which shows significantly improved trajectory following performance.

cs.RO↗

pyrtklib: An open-source package for tightly coupled deep learning and GNSS integration for positioning in urban canyons

Artificial intelligence (AI) is revolutionizing numerous fields, with increasing applications in Global Navigation Satellite Systems (GNSS) positioning algorithms in intelligent transportation systems (ITS) via deep learning. However, a significant technological disparity exists as traditional GNSS algorithms are often developed in Fortran or C, contrasting with the Python-based implementation prevalent in deep learning tools. To address this discrepancy, this paper introduces pyrtklib, a Python binding for the widely utilized open-source GNSS tool, RTKLIB. This binding makes all RTKLIB functionalities accessible in Python, facilitating seamless integration. Moreover, we present a deep learning subsystem under pyrtklib, which is a novel deep learning framework that leverages pyrtklib to accurately predict weights and biases within the GNSS positioning process. The use of pyrtklib enables developers to easily and quickly prototype and implement deep learning-aided GNSS algorithms, showcasing its potential to enhance positioning accuracy significantly.

cs.LG↗

RELEAD: Resilient Localization with Enhanced LiDAR Odometry in Adverse Environments

LiDAR-based localization is valuable for applications like mining surveys and underground facility maintenance. However, existing methods can struggle when dealing with uninformative geometric structures in challenging scenarios. This paper presents RELEAD, a LiDAR-centric solution designed to address scan-matching degradation. Our method enables degeneracy-free point cloud registration by solving constrained ESIKF updates in the front end and incorporates multisensor constraints, even when dealing with outlier measurements, through graph optimization based on Graduated Non-Convexity (GNC). Additionally, we propose a robust Incremental Fixed Lag Smoother (rIFL) for efficient GNC-based optimization. RELEAD has undergone extensive evaluation in degenerate scenarios and has outperformed existing state-of-the-art LiDAR-Inertial odometry and LiDAR-Visual-Inertial odometry methods.

cs.RO↗

CoLRIO: LiDAR-Ranging-Inertial Centralized State Estimation for Robotic Swarms

Collaborative state estimation using different heterogeneous sensors is a fundamental prerequisite for robotic swarms operating in GPS-denied environments, posing a significant research challenge. In this paper, we introduce a centralized system to facilitate collaborative LiDAR-ranging-inertial state estimation, enabling robotic swarms to operate without the need for anchor deployment. The system efficiently distributes computationally intensive tasks to a central server, thereby reducing the computational burden on individual robots for local odometry calculations. The server back-end establishes a global reference by leveraging shared data and refining joint pose graph optimization through place recognition, global optimization techniques, and removal of outlier data to ensure precise and robust collaborative state estimation. Extensive evaluations of our system, utilizing both publicly available datasets and our custom datasets, demonstrate significant enhancements in the accuracy of collaborative SLAM estimates. Moreover, our system exhibits remarkable proficiency in large-scale missions, seamlessly enabling ten robots to collaborate effectively in performing SLAM tasks. In order to contribute to the research community, we will make our code open-source and accessible at \url{https://github.com/PengYu-team/Co-LRIO}.

cs.RO↗

ECMD: An Event-Centric Multisensory Driving Dataset for SLAM

Leveraging multiple sensors enhances complex environmental perception and increases resilience to varying luminance conditions and high-speed motion patterns, achieving precise localization and mapping. This paper proposes, ECMD, an event-centric multisensory dataset containing 81 sequences and covering over 200 km of various challenging driving scenarios including high-speed motion, repetitive scenarios, dynamic objects, etc. ECMD provides data from two sets of stereo event cameras with different resolutions (640*480, 346*260), stereo industrial cameras, an infrared camera, a top-installed mechanical LiDAR with two slanted LiDARs, two consumer-level GNSS receivers, and an onboard IMU. Meanwhile, the ground-truth of the vehicle was obtained using a centimeter-level high-accuracy GNSS-RTK/INS navigation system. All sensors are well-calibrated and temporally synchronized at the hardware level, with recording data simultaneously. We additionally evaluate several state-of-the-art SLAM algorithms for benchmarking visual and LiDAR SLAM and identifying their limitations. The dataset is available at https://arclab-hku.github.io/ecmd/.

cs.RO↗

Tightly Joining Positioning and Control for Trustworthy Unmanned Aerial Vehicles Based on Factor Graph Optimization in Urban Transportation

Unmanned aerial vehicles (UAV) showed great potential in improving the efficiency of parcel delivery applications in the coming smart cities era. Unfortunately, the trustworthy positioning and control algorithms of the UAV are significantly challenged in complex urban areas. For example, the ubiquitous global navigation satellite system (GNSS) positioning can be degraded by the signal reflections from surrounding high-rising buildings, leading to significantly increased positioning uncertainty. An additional challenge is introduced to the control algorithm due to the complex wind disturbances in urban canyons. Given the fact that the system positioning and control are highly correlated with each other, for example, the system dynamics of the control can largely help with the positioning, this paper proposed a joint positioning and control method (JPCM) based on factor graph optimization (FGO), which combines sensors' measurements and control intention. In particular, the positioning measurements are formulated as the factors in the factor graph model, such as the positioning from the GNSS. The model predictive control (MPC) is also formulated as the additional factors in the factor graph model. By solving the factor graph contributed by both the positioning factor and the MPC-based factors, the complementariness of positioning and control can be fully explored. To guarantee reliable system dynamic parameters, we validate the effectiveness of the proposed method using a simulated quadrotor system which showed significantly improved trajectory following performance. To benefit the research community, we open-source our code and make it available at https://github.com/RoboticsPolyu/IPN_MPC.

cs.RO↗

Trajectory Smoothing Using GNSS/PDR Integration Via Factor Graph Optimization in Urban Canyons

Accurate and smooth global navigation satellite system (GNSS) positioning for pedestrians in urban canyons is still a challenge due to the multipath effects and the non-light-of-sight (NLOS) receptions caused by the reflections from surrounding buildings. The recently developed factor graph optimization (FGO) based GNSS positioning method opened a new window for improving urban GNSS positioning by effectively exploiting the measurement redundancy from the historical information to resist the outlier measurements. Unfortunately, the FGO-based GNSS standalone positioning is still challenged in highly urbanized areas. As an extension of the previous FGO-based GNSS positioning method, this paper exploits the potential of the pedestrian dead reckoning (PDR) model in FGO to improve the GNSS standalone positioning performance in urban canyons. Specifically, the relative motion of the pedestrian is estimated based on the raw acceleration measurements from the onboard smartphone inertial measurement unit (IMU) via the PDR algorithm. Then the raw GNSS pseudorange, Doppler measurements, and relative motion from PDR are integrated using the FGO. Given the context of pedestrian navigation with a small acceleration most of the time, a novel soft motion model is proposed to smooth the states involved in the factor graph model. The effectiveness of the proposed method is verified step-by-step through two datasets collected in dense urban canyons of Hong Kong using smartphone-level GNSS receivers. The comparison between the conventional extended Kalman filter, several existing methods, and FGO-based integration is presented. The results reveal that the existing FGO-based GNSS standalone positioning is highly complementary to the PDR's relative motion estimation. Both improved positioning accuracy and trajectory smoothness are obtained with the help of the proposed method.

cs.RO↗