Added working matlab code
Clearer function name