Modificate functions in classes EKF y SO to const. And Fast Matching finished