Cooperative navigation based on graph optimization algorithm (GOA) is a promising method for multi-agent systems (MAS) to provide accurate location information. However, this method exhibits limited real-time capability. To address this issue, we propose a sliding window graph optimization algorithm (SW-GOA) based on marginalization theory for cooperative navigation in this paper. The proposed SW-GOA incorporates relative distance information from ultra wide band (UWB) ranging sensors and dead reckoning (DR) information from inertial measurement unit (IMU) 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 complexity increasing over time encountered by conventional GOA methods. By performing Schur complement on information matrix, historical states are marginalized while retaining effective constraints to enhance accuracy. Additionally, we utilize QR (QR decomposition) incremental update method-based incremental optimization to further expedite 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 EKF method, and the computational efficiency is increased by 82.8\% compared with traditional GOA methods.
Discussion(0)
No comments yet. Be the first to comment.