A sliding window graph optimization algorithm based on marginalization theory for cooperative navigation

Cooperative navigation based on a graph optimization algorithm (GOA) is a promising method for multiagent systems to provide accurate location information. However, this method exhibits limited real-time capability. To address this issue, in this paper, we propose a sliding window GOA (SW-GOA) based on marginalization theory for cooperative navigation. The proposed SW-GOA incorporates relative distance information from ultra-wideband ranging sensors and dead reckoning information from inertial measurement units as factor nodes in the factor graph model. We apply marginalization theory to introduce a sliding window (SW) into the graph model, mitigating the problem of increasing complexity over time encountered by conventional GOA methods. By applying a Schur complement to the information matrix, historical states are marginalized while retaining effective constraints to enhance accuracy. Additionally, we utilize incremental optimization based on the QR (QR decomposition) update method to further accelerate the algorithm. Simulations and experiments are conducted to validate the proposed SW-GOA for cooperative navigation. The experimental results show that the positioning accuracy of the proposed method is improved by more than 20% compared with the extended Kalman filter method, and the computational efficiency is increased by 82.8% compared with traditional GOA methods.