Fix a bug in the setting with speed; set the covariance to the diag([1 1 1 1])