KalmanFilter

http://www.mathworks.co.uk/matlabcentral/fileexchange/loadFile.do?objectId=5377&objectType=file

MatlabCode