More work on the tracking algorithm. Prototype almost ready