Initial commit of code to IFAC WC paper.