Discrete LQR Step

Problem

Take one discrete LQR step for the scalar system x_next = a * x + b * u with stage cost Q * x ** 2 + R * u ** 2.

Given the current Riccati value P, compute the gain K = a * b * P / (R + b ** 2 * P), the control u = -K * x, the next state, and the stage cost. Return [u, x_next, cost].

Hint

The optimal gain balances the control cost R against the future value b squared times P.

Test cases

case 1

in: [1,1,1,1,1,1]

expected: [-0.5,0.5,1.25]

case 2

in: [2,0.9,0.5,1,0.5,2]

expected: [-1.8,0.9,5.62]

case 3

in: [0,1,1,1,1,1]

expected: [0,0,0]

Starter codePython

def lqr_step(x, a, b, Q, R, P):
    # Returns [u, x_next, cost]
    # Your code here
    pass

3 test cases · run your code against them in the editor