adding 3/4th of the MATLAB code

Signed-off-by: Man Parvesh Singh Randhawa <manparveshsinghrandhawa@gmail.com>

Signed-off-by: Man Parvesh Singh Randhawa <manparveshsinghrandhawa@gmail.com>