Small optimization of the kalman filter
Finish merging version by Alexis