Old, non-working MATLAB-based Kalman filter