Changed sensor data to vectors, added vector rotation and added more to the EKF