Correction on translation matrix
Adding Jacobian matrix