Add yaw to observationData structure
Use ros Pose message type for storing robot position in the observationData structure