Heavily altered lqg control. It now works