  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.
