Updated runLQR with now stable initial conditions. I tested this a bunch and will be surprised if it breaks again.