A multifrontal QR factorization approach to distributed inference applied to multi-robot localization and mapping
matrix computations ( 3 . ed . ) .