Changed to vector representations and added some more on the EKF