Trig Fix for get_velocity_xy

forgot to sq
updated toy gains