Code from master thesis project by Sebastian Svensson and Dan Thilderkvist, 2015, at the Department of Automatic Control, Lund University, and Combine Control Systems AB, Lund, Sweden.