Sparse Instantiation of Bias Nodes for Factor Graph-based Terrain-referenced Navigation

Abstract

This study presents a factor graph optimization (FGO)-based integration of the terrain-referenced navigation (TRN) and inertial navigation. Particularly, the problem is subject to multiple sources of bias: 1) measurement bias of the radar altimeter, and 2) odometry bias of the inertial navigation system (INS). Considering relatively slower dynamics of bias propagation compared to the INS, the proposed method instantiates bias nodes more sparsely than position nodes, achieving more stable and accurate results. Due to highly ambiguous characteristics of terrain elevation, the TRN is mostly tackled with non-parametric Bayesian estimators, such as the particle filter (PF). However, the PF has a weakness in solving heterogeneously scaled problem, where in this case the bias sources exhibiting small noise characteristics, and the filtering regime cannot explore correlations among historical measurements. We then demonstrate that fixed-lag smoothing of FGO, which optimizes a windowed bundle of factors, can perform as well as or better than the PF approach via multiple stepwise numerical experiments. Moreover, by sparsely instantiating nodes that designate biases, the system dimension is reduced, thus reducing the average computation time by up to 60% compared to the fully connected case.