def lqr_cp(C, c, F, f, x_init, T, n_state, n_ctrl, u_lower, u_upper):
    """Solve min_{tau={x,u}} sum_t 0.5 tau_t^T C_t tau_t + c_t^T tau_t
                        s.t. x_{t+1} = A_t x_t + B_t u_t + f_t
                             x_0 = x_init
                             u_lower <= u <= u_upper
    """
    tau = cp.Variable((n_state+n_ctrl, T))
    assert (u_lower is None) == (u_upper is None)

    objs = []
    x0 = tau[:n_state,0]
    u0 = tau[n_state:,0]
    cons = [x0 == x_init]
    for t in range(T):
        xt = tau[:n_state,t]
        ut = tau[n_state:,t]
        objs.append(0.5*cp.quad_form(tau[:,t], C[t]) +
                    cp.sum(cp.multiply(c[t], tau[:,t])))
        if u_lower is not None:
            cons += [u_lower[t] <= ut, ut <= u_upper[t]]
        if t+1 < T:
            xtp1 = tau[:n_state, t+1]
            cons.append(xtp1 == F[t]*tau[:,t]+f[t])
    prob = cp.Problem(cp.Minimize(sum(objs)), cons)
    # prob.solve(solver=cp.SCS, verbose=True)
    prob.solve()
    assert 'optimal' in prob.status
    return np.array(tau.value), np.array([obj_t.value for obj_t in objs])
Solve min_{tau={x,u}} sum_t 0.5 tau_t^T C_t tau_t + c_t^T tau_t
s.t. x_{t+1} = A_t x_t + B_t u_t + f_t
x_0 = x_init
u_lower <= u <= u_upper
tests/test_mpc.py