Corrected issue with yaw-checking in test 5
Signed-off-by: Darpa Tech 3 <darpa3@SMILe03.(none)>