RHS seems to work now has same output as matlab version