Improbability filtering for rejecting false positives

We describe an approach, called improbability filtering, to rejecting false-positive observations from degrading the tracking performance of an extended Kalman-Bucy filter. Improbability filtering removes false-positives by rejecting low likelihood observations as determined by the model estimates. It offers a computationally fast and robust method for removing this form of white noise without the need for a more advanced filter. We describe an application of the improbability filter approach to extended Kalman-Bucy filters for tracking ten robots and a ball moving at speeds approaching 5 m s/sup -1/ both accurately and reliably in real-time based on the observations of a single color camera. The environment is highly dynamic and non-linear, as exemplified by the motion of the ball which varies from free rolling under friction, to roiling up 45/spl deg/ inclined walls at the boundary, to being manipulated in unpredictable ways by a mechanical apparatus on each robot. The sensing apparatus, a camera and color blob tracking algorithms, suffers from the usual noise, latency, intermittency, as well as from false-positives caused by the misidentification of an observed object with a nonnegligible likelihood.

I. INTRODUCTION HE Kalman-Bucy filter is a widely used algorithm for tracking observable parameters in linear systems under the presence of stochastic noise [4,5]. In short, it has been described as the optimal tracking algorithm [9] for parameters in the presence of Gaussian noise. The Extended Kalman-Bucy Filter (EKBF) extends the algorithm to include non-linear systems whereby the model is linearised about the state estimate [11].
The filter algorithm is based on the fundamental assumption that the parameters of the observations and dynamics have a Gaussian probability distribution. In other words, any noise components in the system are Gaussian in nature. Although noise is commonly not Gaussian in nature, with suitably chosen parameters, the filter is often robust to moderate violations of the Gaussian noise assumption. This paper describes an extension of the EKBF algorithm to remove white noise, in the form of false-positive observations, from degrading the performance of the filter. False-positives, where incorrect observations are reported with high confidence, violate the Gaussian assumption underlying the EKBF. Modifying the EKBF parameters can compensate for false-positives but at the expense of tracking ability. We describe a new approach, called Improbability Filtering (ImpF), developed to overcome this problem. ImpF uses the state and co-variance estimates of the EKBF to filter out unlikely observations, thereby removing false-positives and enabling the EKBF to track reliably. We describe an application of this algorithm to small size robot soccer where the tracking module must reliably track ten fast moving robots and the ball at the camera frame rate (~60Hz). The highly dynamic nature of the environment, the presence of falsepositives caused by misidentifications, and the need for fast computations provide an ideal testing grounds for ImpF.
The following section briefly describes the robot soccer application that is the basis for the tracking system. Section III describes the EKBF's that we implemented to track different objects and section IV describes the improbability filter algorithm, its approach and implementation along with the results of its performance. In section V, we discuss these results and conjecture where the improbability filter could be used in other applications. Section VI concludes the paper.

II. TRACKING IN ROBOT SOCCER: THE PROBLEM
RoboCup robot soccer is an exciting and challenging robot domain aimed at advancing robot intelligence and control [7]. This paper focuses on the tracking module implementation in a small size robot team where two teams of five robots compete autonomously in a game of soccer with an orange golf ball on a carpeted, ping-pong table-sized field with 45° inclined walls along the boundaries [1]. Figure 1 shows the system structure with an illustration of two robots from either team. Each robot receives radio commands from an off-field PC. The PC generates the commands based on its strategic and tactical algorithms, which in turn derive information about the world from an overhead camera. The overhead camera provides interlaced fields at a rate of 60Hz. Each field is processed using the CMVision color recognition system [2] and the results are reported along with an estimated confidence measure. Thus, each object is reported as: θ ⇒ Here C obs is the confidence of the reported observation over the range 0.0 to 1.0, j refers to the object being tracked be it one of the ten robots or the ball. For the opponent robots and the ball the orientation θ obs has no meaning. Improbability Filtering for Rejecting False Positives T Vision sensing suffers from a number of the usual problems associate with sensors. Data is noisy, intermittent, and has significant latency. Moreover, robot covers have patterns for individual robot identification which when viewed by the overhead camera are occasionally misidentified with one another causing the reported robot locations to jump around between frames. Misidentifications, or false-positives, act as a white noise source on top of the usual Gaussian noise in the sensing stage. Such white noise causes havoc with many tracking algorithms due to the deviations from the Gaussian noise pattern that they cause.
To accurately control the robots at speed, the tracking system needs to account for the deficiencies in the vision output. Additionally, it needs to derive velocity information that cannot directly observe. Finally, it needs to provide facilities to predict, with appropriate confidence measures, where the objects on the field will be at some given time in the future. Although there are many statistical tracking algorithms that have been developed, the Kalman-Bucy filter has proved to be one of the most capable filters. The KBF and its extended variant for EKBF non-linear systems, are widely used [3], have a solid and well-understood mathematical basis, and are computationally tractable when used in such a system as the one discussed here. As such, the EKBF represents a good choice for a tracking mechanism and was the one used for the robots described in this paper and is discussed next.

III. TRACKING WITH THE KALMAN-BUCY FILTER
Each of the eleven objects, two teams of five robots and the ball, is tracked using an independent EKBF. Multiple EKBF's are used due to the non-linear nature of the tracking problem. Robot kinematics are inherently non-linear as is the motion of the ball both along the inclined walls and along the field surface. Multiple independent EKBF's are used based on the assumption that the observations of different objects are independent. This is a reasonable assumption that helps preserve limited computational resources. There are two filter types; the robot EKBF and the ball EKBF, where the difference is due to the different dynamics of the object being tracked. We briefly describe the EKBF equations that are the basis for the filters, which are an extension on our previous EKBF's [8]. For a more detailed discussion of EKBF's refer to [3,9,6,11]. The symbols used here are identical to [11].
In the following discussions, the state estimate will be represented by k x and the state covariances by P k . Two stochastic difference equations model the system dynamics and observations, respectively, as: Here z k is an observation of the system at step k, u k is the command input at time k, w and v are random variables with zero mean. The EKBF operates in two stages. The first step is a dynamical update step where the state estimate and its covariances are updated according to the dynamical model of the system as: The super-minus symbol indicates the results are intermediate, pre-observation update, values. EKBF's work by linearizing at each time step about the state estimate, hence, the matrices A k and W k are the process Jacobians at step k of f(.) with respect to the state estimate A and to noise W, respectively. The Jacobians must be recalculated at each time step. The matrix Q is the process noise covariance and is defined a priori based on knowledge of the dynamical noise components in the system.
The second stage of the filter requires incorporating observations of the real system into the state estimate. The measurement update equations are: where K k is the so-called Kalman gain, R k is the measurement noise covariance set a priori, while H k and V k are the measurement Jacobians with respect to the state estimate and noise, respectively. The operation of the two stages of the algorithm enable the state estimate to track the mean of the observations and the variances to represent the confidence range of the estimate.

A. Robot EKBF
To fully estimate the position and velocity of a fully holonomic robot constrained to a 2D plane, we require six state variables as indicated in Figure 2. This state representation is used for all the robot objects. Thus: Given that all the field objects are represented as particles with velocity and orientation, the process update function neglecting process noise is given by: is the rotation matrix given as: The velocity command is u k, the system input, given by: here u k is the delayed command where l is the system latency of 6 frames corresponding to a delay of about 100ms.
The dynamics update equation is developed straight from the kinematics of the system where a point particle moves with velocity v // parallel to its given orientation, v ⊥ perpendicular to this orientation, while rotating at ω. The α term approximately models the slop in the PID servo loops running on the robots. The α term is set to 0.5, which empirically corresponds to the average slop observed in the robots when responding to velocity commands.
Based on this difference equation, the Jacobians for the dynamics update are: Here, 0 n and I n indicate a zero matrix and identity matrix of size n x n, respectively. Finally, the process noise matrix Q: The standard deviations were chosen to reflect the accelerations that robots are limited to due to wheel slip. Typically robots can accelerate on the order of 3m.s -2 or so, thus, a working value of 4m.s -2 will robustly describe even the fastest robot. Working from an acceleration of 4m.s -2 , a robot can change its velocity by 66mm.s -1 in the 1 / 60 th of a second between iterations. Hence, σ v was set to 66mm.s -1 for all robot types. The exception to this was the differential drive robot that had its lateral deviation set to 4mm.s -1 corresponding to lateral acceleration of 0.25m.s -2 caused by wheel slip. It was naturally assumed that opponents are capable of omnidirectional motion. For rotation, the standard deviations were set to 0.2 rad.s -1 corresponding to an acceleration of 4m.s -2 at the robot wheel. The exception is for the opponents where no orientation can be reliably observed. Hence opponents have σ ω is set to zero.
Observations are only made on the positions of the objects on the field based on the vision output. Velocities are estimated purely on the observation of positions over time and the properties of the EKBF. The observation equation, neglecting measurement noise is straightforward: This is just the observation reported from the vision module and gives Jacobians and measurement noise as: All robots are treated identically with σ xy set to 25mm and σ θ set to 0.173 rad (~10°) as obtained from empirical measurements of the vision output.
The observation update step is not applied blindly. Falsenegatives, low confidence values in the vision output, are rejected immediately and the sensing update is not performed. The dynamics update is still performed meaning the covariances on the state estimate will grow until a successful observation is reported. While this approach combats falsenegatives robustly, it does not overcome the problem of false-positives, situations where the vision reports confidently, but incorrectly. False-positives, which have a disastrous impact on tracking performance, are solved with the improbability filter discussed in section IV.
The state estimate and covariance matrix need to be initialized to reasonable values to ensure their operation is sensible. Similarly, as robots can be removed or substituted during the game, it becomes necessary to reset the EKBF filter to its default, start up state. EKBF resets are driven by user input. In all reset/initialization cases the state estimate and covariance matrix are set to default values of: In short, the state estimate is set to the first observed location and orientation and the covariance matrix is initialized to the noise in location produced by sensing.

B. Ball EKBF
The ball EKBF is a little different to the robot EKBF's as it has different dynamical properties. The state vector is: Orientation and angular velocity are dropped as they have no meaning for the ball. The ball is not driven, so there is no input command, but it has different motions depending on where it is on the field. When traveling freely along the surface of the carpeted field, the ball travels in a straight line but slows due to friction. If the ball hits an edge of the field, it rolls up and along the 45° inclined walls where friction is comparably negligible.
For normal free rolling, the primary assumption is that friction acts as a constant retarding force against the direction of motion unless the ball is traveling sufficiently slowly, where its velocity decays away quite rapidly. The model is: To use the full Jacobians for these equations would result in an overly complex computation that proves unnecessary for the level of tracking required. The Jacobian becomes: where M is specified above.
The remaining dynamics matrices are as before but are lower dimensional. They are: The standard deviation σ xy was again chosen on empirical observations and had a value of 25mm. The velocity standard deviation σ v was more complicated. To allow the filter to react quickly to ball-robot collisions without any explicit dynamic modeling, the state variances are increased when the ball nears a robot. This was achieved through varying σ v depending on the proximity of the ball to a robot as: Here d is the distance between the ball and the nearest robot and R is the maximum robot width. The near robot value σ R was set to 100mm.s -1 , and the open field value σ 0 was set to 10mm.s -1 .

C. Performance
The EKBF algorithm, described above, tracks the field objects well except when false-positives occur. With the appropriate choice of noise covariance parameters, the filter is able to compensate for the Gaussian noise components present in the sensing process but not the white noise components. Handling intermittency in the sensor data is relatively trivial by only performing the sensing update step whenever that field object is confidently identified. That is, if the confidence of identification for that particular field object is above a preset, empirically determined threshold, the sensing update step is computed. Otherwise the predict update step is the only one computed. Due to the nature of the EKBF equations, the covariance matrix P k values will increase representing the increased uncertainty in the state estimates.
The EKBF algorithm enables one to make smart predictions about the future location of a particular field object. By repeated application of the predict update equations to the current estimate of the state vector for a particular object, one can estimate its future state and covariances enabling one to make future predictions along with their respective likelihood. Indeed, this is the approach used for overcoming the inherent latency in the vision process and by the behavior algorithms used to control the robots. Using the EKBF to predict through the inherent latency of the vision sensor substantially improves the level of achievable control with the robots, however, this is beyond the scope of this paper.

IV. IMPROBABILITY FILTERING
Although the EKBF works well under most conditions, it does have one flaw that severely degrades its performance. Occasionally, the vision system will misidentify a field object. On most occasions such misidentifications occur when attempting to identify robots that are partially occluded, something that occurs regularly in general play. Such falsepositives in the sensing stage have a disastrous impact on the performance of the EKBF causing the state estimate to jump across the field. The impact on the robot control strategies is substantial.
The difficulty is caused by the Gaussian assumption that underlies the EKBF. When an observation differing significantly from the state estimate, as determined from the state covariances, is incorporated into the state estimate, the resulting Gaussian deviates significantly from its prior shape and position. A 1D example of this problem is shown in Figure 3. The left Gaussian represents the initial state estimate, the right Gaussian the false-positive observation. The product of the two, after normalization, gives the middle Gaussian, which is no longer a useful estimate of the true state of the system in the short term. Although the filter robustly recovers after some time, the impact of the short term deviation on the system as a whole remains significant.  There are a number of methods that one could conceive to address the problem. Clearly, a suitably complex multihypothesis tracking algorithm, for example [6], could address this problem. The computational cost, however, would be prohibitive for the application discussed here. Moreover, given the frequent occurrence of false-positives, a simpler method is desirable.
More ad-hoc techniques include increasing the sensing covariances, the R matrix parameters. The drawback though is degraded performance of the EKBF under normal conditions due to its decreased sensitivity to sensor input (the K matrix has smaller values). We developed another solution that filters false-positives from observation updates by added domain knowledge. Our early investigations tried such an approach where if the observation deviated by some fixed value from the previous observation it was rejected and the filter reset. This approach however proved sensitive to parameter choice and is somewhat brittle. The question is: is there a principled way to recognize and reject false-positives that is computationally reasonable? We have devised such a method have called it Improbability Filtering (ImpF).
The ImpF approach is related to multi-modal EKBF in that it uses the current state and covariance estimates and the Gaussian basis of the EKBF to estimate the likelihood of observing the given observation. That is: to determine if an observation should be rejected or accepted, one needs to first calculate the conditional probability density function (pdf) P[z k '|x k ,P k ] evaluated for the given observation and state estimates and then use this likelihood to reject observations with too low a likelihood as false-positives.
The conditional pdf is simply the Gaussian curve (in the ndimensional space of the state vector) with the mean of the state estimate and the covariances of the state matrix where both are transformed into the observation space. Evaluating this Gaussian for the given observation will produce the likelihood of the observation with the current model. Thus: C k is the state covariance matrix transformed to observation space, n is the number of state variables and z' is the observation. If an observation is sufficiently likely, as determined by an empirically fixed threshold set to reject a large majority of false-positive observations, the observation is accepted and the filter is updated as per normal. Otherwise the observation is rejected and the update is skipped as in the false-negative case.
The ImpF algorithm rejects false positives in a computationally efficient manner, but does not harm the robust nature of the EKBF to incorrect state estimates. Skipped observations cause the state covariances values to increase without bound via the dynamical update step. If observations are persistently different from the state estimate, the variances will soon become large enough that any observation will become likely and the new observation will be accepted. Moreover, the large covariances will cause the state estimate to jump immediately to the new observation.

A. Experimental Results
To confirm the correct operation of the ImpF, we conducted experiments in both simulation and the real application. The results reported here focus mainly on the simulation. Results are qualitatively similar for the real application, which cannot be presented here a systematic way as false-positives cannot be generated on demand.
The simulation environment has a rich model of the robot application including the full kinematics of the system, some limited dynamics, and modeling of the latencies involved in the various components of the application. For the purposes of this discussion, it will suffice to argue that the simulator captures all the essential parts of the world bar noise that are required for tracking. Noise, an integral part of tracking problem, was modeled in two ways. Gaussian noise, drawn from a Gaussian pdf, provides the usual small signal noise observed during sensing. It accounts for any small errors introduced from vision artifacts such as pixelization, image blur, and pixel noise, among other things. False-positives, which occur due to misidentifications, were modeled as a Poisson process where the interval time between events (in multiples of frames) was drawn from a Gamma distribution. The Gamma parameter was set such that on average 1s passed between false-positive events. Intermittency was not modeled, as it is not realistic.
The following graphs show the tracking performance comparison between the raw EKBF and the ImpF + EKBF for tracking a ball while it rolls down the field and up a wall. The results for the robot tracking provide qualitatively identical and have not been shown here due to space considerations.
The three graphs in Figure 4 demonstrate the raw EKBF tracking ability in the presence of false-positives. The graphs are, in order: A) the X, Y path of the ball as tracked by the EKBF and as reported by the simulated vision. B) the X component of the EKBF and observations over time C) the distance between the real ball position and the tracked.  The jagged steps in the EKBF output are clearly visible from the distance error graph. There is a one-to-one correlation between the large jumps in error and falsepositives. Although the EKBF does recover quite quickly (in the order of ~250ms) these intermediate jumps significantly degrade the controllability of the system as a whole. The poor quality of the tracking is apparent by the distance error mean of 35.7mm (standard deviation of 70.0mm). Figure 5 shows the tracking performance of the EKBF with the ImpF tracking a rolling ball hitting a wall (graph A) and the distance error (graph B). Note the different scales on the y-axis as compared to Figure 4, graph C. The error plot no longer has the large peaks corresponding to false-positives. In comparison to above, the tracking was significantly improved with a mean error of 4.6mm (standard deviation of 2.5mm). Clearly, the ImpF performs its job as desired and greatly improves the overall robustness of the EKBF to falsepositives.

C) Distance Error
Dist. mm Time (s) Figure 5. The ImpF + EKBF performance with (A) the path of the ball hitting a wall and (B) the distance tracking error.

V. DISCUSSION
The experimental results verify the observed performance of the algorithm during game play, which is difficult to express quantitatively, as robust and accurate. Indeed, the system is able to operate robustly even when faced with significant occlusion and periodic misidentifications. Furthermore, its performance degrades gracefully with degraded vision input. Hence, the improbability filter performs the task it was designed for in both a principled fashion and with virtually no 'tweaking' required. The lack of 'tweaking', an attribute of robust systems more often than not, is emphasized by the rather arbitrary likelihood threshold chosen to reject falsepositives. No real effort was required to tune this parameter for the level of tracking required in this application.
The above results demonstrate that in this particular application ImpF successfully removes false-positives. We conjecture that the ImpF approach is more generally applicable as a computationally light means of removing false-positives in a system that is not multi-modal in nature. Moreover, the approach rejects false-positives without introducing additional latency in observation updates or predictions and does not overly complicate the tracking module as a whole. Although applied here to an EKBF, we believe the approach may be applicable to a wider variety of probabilistic state estimators provided that an estimate of the likelihood of the observing the reported observation given the current estimated state of the system can be made.

VI. CONCLUSIONS
In this paper we have described a novel modification to the standard extended Kalman-Bucy filter approach for detecting and rejecting false-positives that would otherwise have a drastic affect on the tracking performance of the filter. We have implemented the algorithm in a working system that must track multiple robots, traveling at high speeds in a confined space, with vision as the primary sensor. Our experimental results clearly demonstrate the performance of the improbability filter approach in rejecting false-positives while not detracting from the otherwise desirable properties of the Kalman-Bucy filter. Future work must now be performed to extend this algorithm to other systems.