A sparsity-aware QR decomposition algorithm for efficient cooperative localization

This paper focuses on reducing the computational complexity of the extended Kalman filter (EKF)-based multi-robot cooperative localization (CL) by taking advantage of the sparse structure of the measurement Jacobian matrix H. In contrast to the standard EKF update, whose complexity is up to O(N⁴) (N is the number of robots in a team), we introduce a Modified Householder QR algorithm which fully exploits the sparse structure of the matrix H, and prove that the overall complexity of the EKF update, based on our QR factorization scheme, reduces to O(N³). Finally, we validate the Modified Householder QR algorithm through extensive simulations, and demonstrate its superior performance both in terms of accuracy and CPU runtime, as compared to the current state-of-the-art QR decomposition algorithm for sparse matrices.