TST: Added end-to-end testing and comparison with the Matlab implementation.