Finished implementing the motion update
Calculated some std deviations
Update variances, incorporate heading better #needtotest
Implement sensor update step of kalman filter
Added timing stuff to the Kalman Filter
Solid/tested implementation of Kalman
About to clean up MMKalman for use in master, but want a commit while it passes tests
Implemented MultiModel Kalman Filter