Added yaw control. Added PI correction using ekf quaternion