Switch updateState function to a C++ version included in Estimator class, small difference in result due to some sort of numerical error