Kalman filter is working for test case
Templated functions to make them easier to use