Modified InvCartPend

Modified SDRE inverted cart pendulum code, got rid of power spectral
density stuff entirely, everything runs in ode45 now