Add SMatrix and tests