Start kalman filter code