Made initialization of Kalman filter more robust