An Algorithm to Compute Averages on Matrix Lie Groups