Add discrete interpollator and fix tests#58

edantec merged 6 commits intomainSimple-Robotics/simple-mpc:mainfrom

topic/discrete_interpollatorSimple-Robotics/simple-mpc:topic/discrete_interpollatorCopy head branch name to clipboard