state estimation code snippets in matlab