Fixed bug with S matrix being calculated incorrectly