main.py 文件源码

def sim(A, B, time, x0, controller):
    x = np.asmatrix(x0)
    prev_y = np.dot(H, x)
    x_out = []
    x_hat_out = []
    u_out = []
    time_out = []

    for t in xrange(time):
        x_hat = x + ((np.random.random((2,1)) - 0.5) * 0.05)
        y = np.dot(H, x_hat)
        u = np.clip(controller(y, prev_y), -40, 40)
        x = np.dot(A, x) + np.dot(B, u)

        x_out.append(x)
        x_hat_out.append(x_hat)
        u_out.append(u)
        time_out.append(t*dt)

        prev_y = np.copy(y)

    return (np.asarray(time_out), np.asarray(x_out), np.asarray(x_hat_out), np.asarray(u_out))