matlab implementation for testing purpose