Optimization now converging.

I switched to a model that defines the nominal position as up in stead of side
ways and added a spring and damper to keep the cart tied to the origin.

I added the discretization interval to the objective function calculation and
set the sample rate to 500 hz and started to get convergance to the correct
gains. The lateral force is now a sum of sines.