Fixed-wing state level HIL via factor graph incremental smoothing

Many robotics problems such as simultaneous localization and mapping (SLAM) and bundle adjustment (BA) can be formulated as a nonlinear least squares graph optimization. In these strategies, each node in the graph represents a robot position or a sensor measurement obtained at that position. Edges represent constraints between these nodes. The first part of the problem known as front-end is generating the graph. The second part, known as back-end, aims to find a configuration that best explains the constraints usually modeled as error functions. Clearly this is an expensive operation since every movement or measurement adds nodes to the graph that may become optimization hard for limited real-time applications. In order to overcome this problem, an innovative method called incremental smoothing, optimizes only a small subset of the graph nodes, reducing the computational load considerably. Besides, to explore this novel algorithm we propose a state-level hardware-in-the loop (HIL) for a small fixed-wing UAV. It consists of an aerial flight simulator extended with our sensor fusion plugin and the UAV aerodynamic model, an autopilot and a ground control station. MAVLink communications protocol interfaces these modules. Furthermore, we validate our approach with three mission scenarios exploiting different maneuvers ranging from a simple square flight to a much more challenging lawn-mower search pattern. Experiments have shown that the proposed method performed satisfactorily.