Fixed a bug in updateState.m and increased max track length to 1000. Seems to perform better now.