Started implementing controller and estimator. Need to figure out why the ode solver is having trouble with solving for this.