Fixes and test to calculate correctly sized residuals/jacobian