Working on HPMPC MHE solver