Add in augmented matrix solving