Added first implementation of KalmanFilter, based boost::numeric::ublas.

Tests were adjusted to new interface of KalmanFilter.