Writing a Custom Function in Matlab