Works now with some LQR iterations.

Simple experiments only.