fixed few bugs in Kalmanestimator