first working version of Kalman
Merge branch 'LocalWork'