Bug fixes and attempts to improve performance
Merge branch 'ukf' of https://github.com/jlfranco/gts into ukf