Initial version of Kalman filter (unverified)