SLAM updated

Working. All returns are NumPy arrays instead of matrices. Better
documentation.