1st draft of MATLAB Kalman engine. Not reliable, but its there