Fixed jacobian calculation so dtheta is added instead of subtracted. Also do some misc. stuff.