First part of predict fcn
Completed predict function (untested)
Added update function for the UKF
Merge branch 'colortrack' of https://github.com/jlfranco/gts into colortrack