Use built-in sqrtm rather than matsqrt
Use .+ instead of + where appropriate