removed startup goal
some changes
yaw using mag