add MatLab implementation for reference