Updated pos and posTest, added vel and velTest