My matrix A is a 6x6 matrix and my matrix B is a 6x1 matrix. I tried several different gains and no gain ever works.
The setpoint is a certain position where my system should stabilize.
class PIDController: def __init__(self, Kc, Ti, Td, setpoint, A, B, C, D): #initializing, for example: def compute(self, current_value, current_time): error = self.setpoint - current_value[0] delta_time = current_time - self.prev_time self.prev_time = current_time # Proportional term proportional = self.Kc * error self.proportional_values.append(proportional) # Integral term self.integral += error * delta_time self.integral_values.append(self.integral) # Store integral term # Derivative term self.derivative = (error - self.prev_error) / delta_time self.derivative_values.append(self.derivative) self.prev_error = error pid_output = proportional + self.Kc/self.Ti * self.integral + self.Kc * self.Td * self.derivative # State-space model simulation # x_next is most important. output is just a chosen var from via the C matrix x_next = np.dot(self.A, current_value) + self.B * pid_output output = np.dot(self.C, current_value) + self.D * pid_output self.r_values.append(current_value[0]) self.error_values.append(error) return output, x_nextI tried adjusting gains ranging from 0.001 - 1000. My system will oscillate for around 6-7 iterations and will then go to infinity very quickly.