Updated

Fixed a matrix multiplication error, code works correctly.