class Controller:
def __init__(self, Kp, Ki, n, A, B, C, Q, R):
self.Kp = Kp
self.Ki = Ki
self.u = 0.0
self.err = 0.0
self.kalman = Kalman(n, A, B, C, Q, R)
def compute_next_u(self, y_err):
(x_out, y_out) = self.kalman.compute_kalman(self.u, y_err)
self.err = (self.err - y_out)[0, 0]
self.u = (self.Ki * self.err - self.Kp * x_out)[0, 0]
return self.u
