-
Notifications
You must be signed in to change notification settings - Fork 0
BallEKF
The vision system produces highly noisy measurements which, taken alone, are too unstable to be used in behaviors. To reduce the noise in the ball measurements, while still accurately tracking the ball's movements, we employ an Extended Kalman Filter.
The Northern Bites EKF class is a template of 4 parameters: MeasurementType, UpdateModel, dimension, and measurementSize.
The BallEKF takes a RangeBearingMeasurement for its "correction step." The measurements of the ball consist of a distance and bearing estimate to the ball on screen.
The BallEKF's "update step" takes a MotionModel object. The MotionModel class encapsulates a robot's motion over one frame. The delta member variables (F,L,R) store the calculated forward (x-axis), lateral (y-axis), and rotational change.
This variable represents size of the EKF's StateVector. The state of the BallEKF is a 6-dimensional vector incorporating knowledge of the ball's (x,y) position, (x,y) velocity, and (x,y) acceleration.
An EKF is a two step filter. First, the timeUpdate occurs. Based on internal state, and the robot odometry provided by the MotionModel object, the BallEKF updates its internal StateVector. The updatePosition, Velocity, and Acceleration methods update their respective state components using the changes in the robot's odometry and the last calculated velocity and acceleration plus a friction component.
The correction step uses the RangeBearingMeasurement to calculate V_k (the difference between our observed ball position and the EKF's internal StateVector) and R_k (the covariance matrix of the RangeBearingMeasurement observation). During this step, the BallEKF also calculates the standard error to determine if the observation is too divergent from the model to be incorporated. This helps the system ignore highly erroneous measurements.
The BallEKF keeps a count of the erroneous measurements recorded. When the count grows above a threshold, the EKF is reset to the next observed location. This helps to solve the "teleporting ball problem" when a ball is picked up by a referee or kicked too quickly to see by a robot.