Fixing unit-test for LQR

This unit-test uses the spring-damper system. So first we need to work on issue #3 (closed).