This is the last version before a complete program rewrite. All functions are working as expected but the inverse jacobian isn't giving the expected results as compared to the Matlab function.