This is a test of the AIBO's tracking system.  It's mainly a place
where I (Paul Rybski) can hack on my tracking code outside of the
dogs.  There's no guarantee that this stuff will compile at any given
time.  :)

---

TODO -- matrix

Use Scott's new determinant() function from vision

---

TODO -- World Model

Create a tracker of observations from other robots.
  - If an external observation is close to one of our own
    observations, then we update with the covariance but don't change
    the mean.  How do we merge them properly?

Change the world model interface for AIBO observations to fill a
vector of Gaussians for robot positions instead of just a single
Gaussian, especially if there are multiple independent observations in
the same timestep.

(Research) 
Look at how the mean and covariance of the localization system's
estimate is being computed.  Figure out how best to fit a Gaussian (a
mixture of Gaussians would be better to this distribution).  If there
are multiple Gaussian means, then we should potentially consider
making use of them all for teammate ball position estimates.

---

TODO -- Ball Tracker

Change the estimate so that the balls are kept around longer.  If we
have more than one hypothesis, we make the others disappear faster.

---

TODO -- Teammate Ball Tracker

Change the estimate so that there must be at least two balls that are
right on top of each other before we use it.

---

TODO -- Multi KF Pos

The many different process models must be taken into account.
- Injection of our own actions: don't lose the ball just because we've
  lifted our head!  I need a method that I can call to tell me whether
  our camera can view the ball but that our body can see it.
- This can be done in Vision somehow

3 important cases where we model the process noise of the ball

- ball is stationary
- ball has velocity
- ball is there because we kicked it
- ball is there because someone else kicked it

1) encorporate the vision confidence, especially for distance
2) make the covariance growing and shrinking very aggressive.
3) when we have the observations from other robots, if they are close
   enough not to make multipe hypothesis, we _use_the_covariances_ but
   don't change the means.  We want to focus on emphasizing the
   observations that we have now.

Make the kalman filter classes generic in name (get rid of the 'Ball'
prefix) so that other things can be tracked.

(research)
Use the kalman class for the KFPos

(research)
Try to handle ellipses that are different shapes (good for angular
values, bad for distance) than circular.

(reesarch)
Add IR to the tracker

---

(research)
TODO -- EKF Pos Vel tracker

Rewrite the EKF so that it takes a velocity _observation_ as the
input.  We'll do the indirect EKF (KF?) thing and track velocity by modeling
an outside.

EKF would take into account the robot's position and do the tracking
based on the transformation from local to odometric coordinates.  Try
both the KF tracker where the odometric coordinate system is already
taken into account, as well as the EKF tracker where the local
measurements are the only things that are used.

Add IR to the tracker

Test the Kalman filter against outliers.  
  * Play with the sensor covariance -- base it on empirical evidence
  * Play with the model covariance
  * Add improbability filtering
  * Examine the spurrious ball problem
  * Do an exponential decay friction model - update the Jacobians

Build a simulator to test out the Kalman Filter under controlled
circumstances

Do EKF with the robot's pose as part of the state vector.
  * MotionLocalizationUpdate is where we can get the velocity updates

Do we track the ball in odometric and global coordinates?  How about
both and use both as multiple hypotheses?

Think about the derivation of the EKF and think about ways of modeling
it better
  * Compute the Jacobian with the friction model
  * Consider the problem of estimation with 

Tune the filter for sensor covariance R and plant covariance Q
  * Measure the variance in the sensor readings 
  * Come up with different cases for propagation covariance
    * When the ball goes slowly, it will arc and sometimes curve back
      on itself
  * Change the friction model to be more realistic

Add reasoning about the walls to the Kalman Filter model.  
  * Capture the ball position in local coordinates and convert to odo
  * Return the position in world coordinates

Figure out when to reset the Kalman filter

Add methods to WorldModel for different levels of behavioral use
  * Add methods for returning whether the ball is known or known based on
    some threshold
  * If multiple estimates are available, give users the option of
    looking at them if they so desire

Add methods for merging readings from different teammates

Add a multi-hypothesis tracking system
  * Add new Gaussians to the mix as needed (up to 'n' where 'n' is small?)
  * Think about a Rao-Blackwellized PF
  * Track different models of motion as needed

Add potentially varible-length latency between frames (track the true fps)

Remove the information matrix from the extendedkalman filter to save on
computation and memory (we're not using it right now anyway...)

