Fixed critical bug in velocity observation, implemented yaw controller, working autopilot stabilizer