clean up old cruft, EKF code
calculate mean and stddev of swarm as a measure of convergence
add finalize, move semaphore as early as possible