MPC Horizon Cost

Problem

Evaluate the finite-horizon MPC cost for a state and control sequence.

Return sum over t of (Q * xs[t] ** 2 + R * us[t] ** 2) plus the terminal term Qf * xs[n] ** 2, where n = len(us) and xs has n + 1 entries.

Hint

The terminal state costs Qf but has no control term.

case 1

in: [[1,0.5,0],[-1,0],1,1,1]

expected: 2.25

case 2

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

expected: 11.125

case 3

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

expected: 0

Starter codePython

def mpc_horizon_cost(xs, us, Q, R, Qf):
    # Your code here
    pass

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