update_0.9: Code overhaul, change to sensors coding
			Changed the code to allow for yaw consideration