Fixed the bug that was causing Kalman Filter update to screw up due to ordering of duplicate observations.