Fix bug with matrix calculation and turning