Fixed algorithm training and testing. Now produces same output as Matlab code.