Add test position with weight
Quicker founding of yaw angles