made some modifications in kalman