first Kalman Filter code that apparently works