Robust phased-array radio system aided inertial navigation using factor graph optimisation

Global navigation satellite systems (GNSS) is the gold standard for aiding of inertial navigation systems (INS), but is highly susceptible to naturally occurring, unintentional, and intentional interference due to its low signal power. GNSS is also not suitable for use indoors or in other areas where the signal is blocked. In recent years, phased-array radio systems (PARS) have shown to have potential for navigation. After Bluetooth introduced direction finding in its specification, the technology has emerged as a low-cost PARS-based alternative. In this paper, an estimation scheme fusing PARS and inertial sensor data based on factor graph optimisation (FGO) using incremental smoothing and mapping with fixed-lag smoothing is applied and compared against a standard error-state Kalman filter (ESKF) solution as a benchmark. The results are obtained from 100 Monte Carlo runs of a simulated USV trajectory, where performance is compared when aided by both fault-free and erroneous PARS measurements, with and without robust estimation schemes. We find that our estimator slightly outperforms ESKF in North-East position and roll/pitch estimates, but struggles more with Down-position and yaw angle, possibly due to additional cross-covariance available to the ESKF.