Added covariance propagation and modified Jacobian functions so that we can just pass in the state{k} struct instead of state and k