Distributed collaborative navigation for UAV formation based on factor graph optimization

The global navigation satellite system (GNSS) positioning accuracy is highly degraded due to signal reflections from buildings. A stand-alone receiver only has limited measurement capabilities, thus resulting in limited performance. With the rapid development of communications techniques, sharing measurements between receivers have become possible. Therefore, this study proposed a novel 3D mapping aided (3DMA) GNSS-based cooperative positioning method that makes use of all the available surrounding receivers’ measurements for acquiring better positioning solutions. By complementarily integrating the GNSS ray-tracing algorithm and the double difference technique, the uncorrelated errors are mitigated while eliminating the correlated errors between users. As a result, a more accurate relative positioning solution is achieved, which can further improve the absolute positioning accuracy of the degraded receiver. To improve the robustness of the cooperative positioning algorithm, factor graph optimization is employed to obtain an overall optimal positioning solution among multiple receivers’ solutions. By further integrating the 3DMA GNSS cooperative positioning with factor graph optimization, the positioning accuracy and robustness are improved, and the method achieves a root mean square error of less than 10 meters for most receivers in a dense urban area.

For simultaneous localization and mapping (SLAM) in large-scale scenarios, the influence of long-distance and high-speed motion cannot be ignored because the risk of huge odometry drift will increase. To solve this problem, we propose the LiDAR -inertial odometry (LIO) method via Kalman filter and factor graph optimization (LIO-FILO), which provides real-time, high-frequency, and high-precision odometry. The LIO system consisting of three modules is established to fit various application scenarios. The state estimation module provides the pose states to be optimized and receives timely feedback from the pose optimization module. In the loop closure module, LIO-FILO constructs a multilayer structure loop closure detection method, including different schemes of detection, and the loop closure factor is constructed by the pose transformation matrix calculated by ICP. In the pose optimization module, the fast-build adjacent constraint factors and the loop closure factors are added to the factor graph to get the optimized result based on the GTSAM library. The real-world experiments show that LIO-FILO can mitigate the odometry drift to achieve accurate mapping results and obtain higher precision odometry compared with the existing advanced SLAM methods.

This paper proposes a robotic state estimation and map construction method. The traditional lidar SLAM methods are affected by sensor measurement noise, which causes the estimated trajectory to drift, especially along the altitude direction caused by lidar noise. In this paper, ground parameters in the environment are extracted to construct the ground factors to compress the trajectory estimation drifting along the altitude direction using the characteristics of constant robot pose relative to the ground. Our method uses tightly coupled lidar and inertial to obtain low-drift lidar odometry factors by factor graph optimization. The optimized lidar odometry factors are then added to a global factor graph, together with ground, loop closure, and GPS factors to obtain accurate robot state estimation and mapping after factor graph optimization. The experimental results show that our method has comparable results with advanced lidar SLAM methods, and even performs better in some complex and large-scale environments.

This paper proposes a new method for RTK post-processing. Different from the traditional forward-backward Kalman filter, in our method, the whole system equation is built on a graphical state space model and solved by factor graph optimization. The position solution provided by the forward Kalman filter is used as the linearization points of the graphical state space model. Constant variables, such as double-difference ambiguity, will exist as constants in the graphical state space model, not as time-series variables. It is shown by experiment results that factor graph optimization with a graphical state space model is more effective than Kalman filter with a traditional discrete-time state space model for RTK post-processing problem.

Substations are critical infrastructures in modern power systems, requiring accurate and reliable personnel positioning to ensure operational safety and inspection efficiency. Pedestrian dead reckoning (PDR) technology has emerged as a preferred solution for real-time seamless positioning due to its autonomous, anti-jamming and passive characteristics. However, in practical applications, the PDR algorithm faces challenges such as the inability to provide absolute positioning due to unknown starting points and cumulative errors over time. To address these challenges, this paper proposes a head-mounted pedestrian positioning system (HPPS) integrating fisheye camera/real-time kinematic (RTK)/PDR based on factor graph optimization (FGO). First, a skyward-facing fisheye camera identifies non-line-of-sight (NLOS) signals caused by architectural obstructions, improving the absolute positioning accuracy of RTK. Next, an adaptive pedestrian gait detection and step length estimation algorithm based on head-mounted inertial measurement units (IMU) is developed to enhance PDR robustness. Finally, the FGO framework integrates the inputs of the fisheye camera, RTK, and PDR to achieve seamless indoor-outdoor positioning. Experimental results demonstrate that the proposed system achieves horizontal root mean square error (RMSE) values of less than 0.42 m and 0.35 m in two distinct outdoor substation environments and less than 1.36 m in indoor-outdoor scenarios. This work highlights the integration of precise positioning technologies within the Internet of Things (IoT) framework, advancing smart grid applications and enabling effective location tracking in complex environments.

Global navigation satellite systems (GNSSs) applied to intelligent transport systems in urban areas suffer from multipath and non-line-of-sight (NLOS) effects due to the signal reflections from high-rise buildings, which seriously degrade the accuracy and reliability of vehicles in real-time applications. Accordingly, the integration between GNSS and inertial navigation systems (INSs) could be utilized to improve positioning performance. However, the fixed GNSS solution uncertainty of the conventional integration method cannot determine the fluctuating GNSS reliability in fast-changing urban environments. This weakness becomes solvable using a deep learning model for sensing the ambient environment intelligently, and it can be further mitigated using factor graph optimization (FGO), which is capable of generating robust solutions based on historical data. This paper mainly develops the adaptive GNSS/INS loosely coupled system on FGO, along with the fixed-gain Kalman filter (KF) and adaptive KF (AKF) being taken as comparisons. The adaptation is aided by a convolutional neural network (CNN), and the feasibility is verified using data from different grades of receivers. Compared with the integration using fixed-gain KF, the proposed adaptive FGO (AFGO) maintains the 100% positioning availability and reduces the overall 2D positioning error by up to 70% in the aspects of both root mean square error (RMSE) and standard deviation (STD).

This paper explores the pervasive challenges of pedestrian positioning using smartphones in densely populated urban environments where Global Navigation Satellite System (GNSS) signals are inaccessible, for example, in indoor areas. Existing sensor-based positioning methods, such as inertial navigation systems (INS), GNSS, and visual-inertial odometry (VIO), suffer from inherent restrictions that compromise the accuracy and reliability of the positioning performance. An approach based on machine learning is proposed to address these limitations, employing the Support Vector Machine (SVM) algorithm to accurately distinguish indoor/outdoor (IO) based on the measurement of GNSS. The proposed approach in this study seamlessly incorporates 3D mapping aided (3DMA) GNSS measurements and localized estimations derived by VIO via factor graph optimization (FGO), complemented by an IO detection switch, to achieve accurate pose estimation and effectively eliminate global drift. The system's effectiveness and robustness are rigorously assessed through comprehensive extensive real-life experiments, with an average reduction of 4 meters, leading to noteworthy and statistically significant findings. Received: 1 April 2024 | Revised: 13 September 2024 | Accepted: 26 October 2024 Conflicts of Interest The authors declare that they have no conflicts of interest to this work. Data Availability Statement The source code that supports the findings of this study is openly available in 3DMAGNSSVINS-IOFGO at https://github.com/queenie-ho/3DMAGNSSVINS-IOFGO. Author Contribution Statement Hiu-Yi Ho and Hoi-Fung Ng: Conceptualization, Methodology, Software, Validation, Formal analysis, Investigation, Data curation, Writing - original draft, Writing - review & editing, Visualization. Weisong Wen: Conceptualization, Resources, Writing - review & editing, Supervision, Project administration, Funding acquisition. Yanlei Gu: Resources, Writing - review & editing, Writing - review & editing. Li-Ta Hsu: Conceptualization, Resources, Writing - review & editing, Supervision, Project administration, Funding acquisition.

Extended Kalman Filter (EKF) is extensively employed in Global Navigation Satellite System (GNSS)-based real-time orbit determination (RTOD) for microsatellites due to its low complexity. However, the performance of EKF-RTOD is markedly degraded when the microsatellite deviates from a stable Earth-pointing attitude and employs a low-cost receiver. Factor graph optimization (FGO), which addresses nonlinear problems through multiple iterations and re-linearization, has demonstrated superior accuracy and robustness compared to EKF in challenging environments such as urban canyons. In this study, we introduce a novel FGO-based RTOD (FGO-RTOD) approach, which integrates state transfer factors to establish temporal connections between state nodes across multiple epochs. Real-time processing is achieved through a sliding window mechanism combined with marginalization. This paper evaluates the performance of the proposed algorithm in a regular scenario using data from GRACE-FO-A, which maintains the Earth-pointing attitude and employs a high-performance receiver. The positioning results of GRACE-FO-A indicate that FGO-RTOD marginally outperforms EKF-RTOD in accuracy. Furthermore, the performance of FGO-RTOD is assessed in challenging scenarios using simulation data and on-orbit data from Tianping-2B microsatellite, which is not in an Earth-pointing attitude and employs a low-cost receiver. The simulation results reveal that FGO-RTOD reduces the Root Mean Square (RMS) of positioning error by 79.0% relative to EKF-RTOD and exhibits significantly enhanced smoothing. In the Tianping-2B experiments, FGO-RTOD reduces the RMS of carrier-phase ionosphere-free combination residuals from 2 cm to 1 cm relative to EKF-RTOD, alongside a substantial improvement in the ratio of valid observations. These findings underscore the effectiveness of FGO-RTOD in managing outlier measurements in challenging scenarios.

In challenging environments, conventional positioning algorithms often exhibit performance degradation due to outlier-contaminated sensor measurements. To address this issue, this paper proposes a robust localization method based on regularization with adaptive scaling (RAS). The proposed method introduces the scaling factor to characterize the amplification of noise variance caused by outliers and integrates them within a probabilistic model. RAS factor graph optimization (FGO) is developed as a robust localization method based on the prior probability distribution of the scaling factor, and is derived from the maximum a posteriori (MAP) estimation to suppress the influence of abnormal measurements. A data-driven parameter calibration approach is also proposed to avoid manual tuning. Theoretical analysis is further conducted to examine the robustness of RAS FGO. The influence of the prior distribution parameters of the scaling factor on the suppression of abnormal measurements loss is investigated. It is also shown that the Weighted Least Squares and the Switchable Constraints methods are special cases of RAS FGO. Experimental results on the Google Smartphone Decimeter Challenge datasets demonstrate that the proposed method significantly outperforms traditional FGO in terms of both accuracy and robustness, particularly in scenarios with frequent measurement anomalies. In representative scenarios, the 3D RMSE decreases from 18.87 m to 4.77 m, which corresponds to a reduction of 74.72%. In addition, the 95% cumulative 3D error decreases from 18.13 m to 8.16 m, representing a reduction of 54.99%.

As the demand for indoor services relying on location information grows, achieving precise indoor positioning is becoming an urgent issue to address. The ultra-wide band (UWB) accuracy can reach centimeter level, but is susceptible to non-line-of-sight (NLOS) effects. The inertial navigation system (INS) is not affected by the environment, but the error will diverge over time. Therefore, this paper focuses on the UWB/INS combined positioning system, which can combine the advantages of both. We propose a loosely-coupled INS/UWB indoor localization scheme based on factor graph optimization (FGO). Firstly, an agglomerative hierarchical clustering based NLOS mitigation method is applied to preprocess the UWB raw measurement data. The position estimates under UWB are then obtained using an improved extended Kalman filter to mitigate NLOS errors and provide more accurate location data for combined navigation. Then, we build the IMU preintegration factor and the zero-bias factor. Finally, FGO is used to fuse the information to get the final location. Designed experiments demonstrate the superiority of the algorithm. The simulation and experimental results show that the proposed scheme outperforms the comparison algorithms.

Multi-sensor fusion and precise point positioning real-time kinematic (PPP-RTK) techniques are gaining popularity in vehicle navigation studies. The integration of the Global Navigation Satellite System (GNSS), inertial navigation system (INS), and vision can significantly improve the accuracy and continuity of positioning. The methods used to integrate different measurements are usually divided into an extended Kalman filter and factor graph optimization (FGO). We propose an integration method of PPP-RTK/INS/Vision that utilizes both the square-root information filter (SRIF) and FGO, combining the merits of the two algorithms. The proposed PPP-RTK/INS/Vision integration consists of two parts: the first is reweighted PPP-RTK/INS tightly coupled integration based on SRIF, and the second is GNSS/INS/Vision integration based on a two-step FGO method with feature selection in the vision component. The first positioning result is integrated with the FGO-based visual INS with uniform depth distribution to generate the final navigation information. The real-world experiment, carried out in an urban area, includes many GNSS-challenged environments, such as high buildings, tunnels, and viaducts. The results demonstrate that the proposed method achieves a root mean square of 0.57, 0.65, and 0.86 m in the north, east, and down components of the entire trajectory, respectively. Compared to the PPP-RTK/INS/Vision tightly coupled integration with a multi-state constrained Kalman filter, it shows an 11.0% improvement in vertical accuracy and comparable horizontal accuracy. Moreover, in the GNSS-challenged environment, the performance of the proposed method is more outstanding in the vertical component, with an improvement of 83.0% in the vertical direction and similar horizontal accuracy. Our study provides a novel approach to integrating both measurements and optimal estimation methods.

Tightly coupled integrated navigation system via factor graph for UAV indoor localization

In response to the issues of low accuracy, perception degradation, and poor reliability of single-sensor SLAM technologies in complex environments, this study presents a novel IMU-centered multi-sensor fusion SLAM algorithm (IFAL-SLAM) integrating LiDAR, vision, and IMU, based on factor graph elimination optimization (\textbf{I}MU-centered multi-sensor \textbf{F}usion, \textbf{A}daptive \textbf{L}agrangian methods). The proposed system leverages a multi-factor graph model, centering on the IMU, and applies a covariance matrix to fuse visual-inertial and LiDAR-inertial odometries for bias correction, using loop closure factors for global adjustments. To minimize the optimization costs post-fusion, a sliding window mechanism is incorporated, coupled with a QR decomposition elimination method based on Householder transformation to convert the factor graph into a Bayesian network. Finally, an adaptive Lagrangian relaxation method is proposed, employing matrix-form penalty parameters and adaptive strategies to enhance convergence speed and robustness under high rotational dynamics.Experimental results indicate that the proposed algorithm achieves absolute trajectory errors of approximately 0.58 m and 0.24 m in large and small complex scenes, respectively, surpassing classic algorithms in terms of accuracy and reliability.

We present a multi-camera visual-inertial odometry system based on factor graph optimization which estimates motion by using all cameras simultaneously while retaining a fixed overall feature budget. We focus on motion tracking in challenging environments, such as narrow corridors, dark spaces with aggressive motions, and abrupt lighting changes. These scenarios cause traditional monocular or stereo odometry to fail. While tracking motion with extra cameras should theoretically prevent failures, it leads to additional complexity and computational burden. To overcome these challenges, we introduce two novel methods to improve multi-camera feature tracking. First, instead of tracking features separately in each camera, we track features continuously as they move from one camera to another. This increases accuracy and achieves a more compact factor graph representation. Second, we select a fixed budget of tracked features across the cameras to reduce back-end optimization time. We have found that using a smaller set of informative features can maintain the same tracking accuracy. Our proposed method was extensively tested using a hardware-synchronized device consisting of an IMU and four cameras (a front stereo pair and two lateral) in scenarios including: an underground mine, large open spaces, and building interiors with narrow stairs and corridors. Compared to stereo-only state-of-the-art visual-inertial odometry methods, our approach reduces the drift rate, relative pose error, by up to 80% in translation and 39% in rotation.

The escalating development of Maritime Autonomous Surface Ships (MASS) has imposed rigorous demands on the precision, continuity, and resilience of onboard integrated navigation systems. However, in complicated marine settings, data from the Global Navigation Satellite System (GNSS) and Doppler Velocity Log (DVL) are frequently corrupted by multipath effects and non-line-of-sight (NLOS) interference. These disturbances introduce anomalous observations that violate Gaussian noise assumptions, thereby severely deteriorating the robustness and estimation quality of traditional sliding-window factor graph optimization (SW-FGO). To mitigate this problem, this study introduces a novel integrated navigation strategy termed Gradient-Adaptive Factor Graph Optimization (GA-FGO). By designing a gradient-adaptive robust objective function within the factor graph structure, the proposed method dynamically re-weights constraints from the Inertial Navigation System (INS), GNSS, and DVL. This mechanism effectively attenuates the influence of measurement outliers at the optimization level. Furthermore, a unified solution framework utilizing Iterative Reweighted Least Squares (IRLS) and the Gauss–Newton method is established to simultaneously perform adaptive weight updates and state estimation. Validation based on offline field data—benchmarked against the Extended Kalman Filter (EKF), Unscented Kalman Filter (UKF), and standard SW-FGO. Simulation results demonstrated that the GA-FGO algorithm achieves superior positioning accuracy and estimation stability under realistic measurement conditions.