got rid of spiking at jump terms by changing sign in lqr calculation