ALMATH: add function toVector to PositionAndVelocity struc

Add test toVector for all available type

Change-Id: I83ac697b7704591519e9d7d4a7364262bae334fa