The code does seem to give the correct values when compared to the MATLAB code.Need to do more testing to see if it fully works