Got a simple rocket lqr controller working