We wrote some math to track the position of balls on the field. Our calculation takes various inputs from the cameras and sensors, and outputs two coordinates

• BX = The X-coordinate of the Ball in question. Uses common unit, based on the center.
• BY = The Y-coordinate of the Ball in question. ^
• RX = The X-coordinate of the Robot. ^
• RY = The Y-coordinate of the Robot. ^
• HRF = The Heading of the Robot relative to the Field. Angle is relative to the Y axis.
• HBR = The Heading of the Robot relative to the Field. ^
• D = The Distance between the robot and the ball. Uses common unit, based on the center.

$B_X = R_X + \cos( H_{RF} + H_{BR} ) \times D$ $B_Y = R_Y + \sin( H_{RF} + H_{BR} ) \times D$

It's basically a distance equation, A = B + Δ, with some additional trigonometry to squeeze ΔX and ΔY out of angle and direct distance, in millimeters.