Updated dynamics parameters and added measurement innovation check function