def roll_forward(A, B, K, x0, psi0, sigma_w, horizon, rng=None):
    'Apply an LTI controller K = (A_k,B_k,C_k,D_k)\n\n    Roll the true system (A, B) forward with the SS realization of the LTI\n    controller given. horizon is the length of the trajectory, and\n    sigma_w is the stddev of the Gaussian process noise.\n\n    '
    if (rng is None):
        rng = np.random
    _assert_AB_consistent(A, B)
    (A_k, B_k, C_k, D_k) = K
    _assert_ABCD_consistent(A_k, B_k, C_k, D_k)
    (state_dim, input_dim) = B.shape
    psi_dim = A_k.shape[0]
    assert (C_k.shape[0] == input_dim)
    assert (B_k.shape[1] == state_dim)
    if (x0 is None):
        x0 = np.zeros((state_dim,))
    if (psi0 is None):
        psi0 = np.zeros((psi_dim,))
    assert (x0.shape == (state_dim,))
    assert (psi0.shape == (psi_dim,))
    process = (sigma_w * rng.normal(size=(horizon, state_dim)))
    xt = np.array(x0)
    psit = np.array(psi0)
    states = np.zeros(((horizon + 1), state_dim))
    inputs = np.zeros((horizon, input_dim))
    controller_states = np.zeros(((horizon + 1), psi_dim))
    states[0, :] = x0
    controller_states[0, :] = psi0
    for t in range(horizon):
        psitp1 = (A_k.dot(psit) + B_k.dot(xt))
        ut = (C_k.dot(psit) + D_k.dot(xt))
        xtp1 = ((A.dot(xt) + B.dot(ut)) + process[t])
        inputs[t, :] = ut
        states[(t + 1), :] = xtp1
        controller_states[(t + 1), :] = psitp1
        xt = xtp1
        psit = psitp1
    return (states, inputs, controller_states)
Apply an LTI controller K = (A_k,B_k,C_k,D_k)

Roll the true system (A, B) forward with the SS realization of the LTI
controller given. horizon is the length of the trajectory, and
sigma_w is the stddev of the Gaussian process noise.
def roll_forward(A, B, K, x0, psi0, sigma_w, horizon, rng=None):
    'Apply an LTI controller K = (A_k,B_k,C_k,D_k)\n\n    Roll the true system (A, B) forward with the SS realization of the LTI\n    controller given. horizon is the length of the trajectory, and\n    sigma_w is the stddev of the Gaussian process noise.\n\n    '
    if (rng is None):
        rng = np.random
    _assert_AB_consistent(A, B)
    (A_k, B_k, C_k, D_k) = K
    _assert_ABCD_consistent(A_k, B_k, C_k, D_k)
    (state_dim, input_dim) = B.shape
    psi_dim = A_k.shape[0]
    assert (C_k.shape[0] == input_dim)
    assert (B_k.shape[1] == state_dim)
    if (x0 is None):
        x0 = np.zeros((state_dim,))
    if (psi0 is None):
        psi0 = np.zeros((psi_dim,))
    assert (x0.shape == (state_dim,))
    assert (psi0.shape == (psi_dim,))
    process = (sigma_w * rng.normal(size=(horizon, state_dim)))
    xt = np.array(x0)
    psit = np.array(psi0)
    states = np.zeros(((horizon + 1), state_dim))
    inputs = np.zeros((horizon, input_dim))
    controller_states = np.zeros(((horizon + 1), psi_dim))
    states[0, :] = x0
    controller_states[0, :] = psi0
    for t in range(horizon):
        psitp1 = (A_k.dot(psit) + B_k.dot(xt))
        ut = (C_k.dot(psit) + D_k.dot(xt))
        xtp1 = ((A.dot(xt) + B.dot(ut)) + process[t])
        inputs[t, :] = ut
        states[(t + 1), :] = xtp1
        controller_states[(t + 1), :] = psitp1
        xt = xtp1
        psit = psitp1
    return (states, inputs, controller_states)