Kalman filtering and LQR (first pass)