Fixed numerical integration of Jacobian

Changed h(x,u) to correctly call xhatminus, as well as
numericalJacobian. Made numericalJacobian use linear algebra.
Changed UKF to use dt=1
Created inputs of timestep dt=0.1