From bf205742b18f850f5240276635f4a8614a8dff97 Mon Sep 17 00:00:00 2001 From: jens Date: Thu, 26 Nov 2020 19:57:28 +0100 Subject: [PATCH] - added Pid - added Temp Controller - added Kalman --- brewpi.py | 68 +++++--------- components/actor/__init__.py | 0 components/pid/__init__.py | 0 components/pid/kalman.py | 144 ++++++++++++++++++++++++++++++ components/pid/pid.py | 48 ++++++++++ components/pid/temp_controller.py | 41 +++++++++ components/plant/__init__.py | 0 7 files changed, 255 insertions(+), 46 deletions(-) create mode 100644 components/actor/__init__.py create mode 100644 components/pid/__init__.py create mode 100644 components/pid/kalman.py create mode 100644 components/pid/pid.py create mode 100644 components/plant/__init__.py diff --git a/brewpi.py b/brewpi.py index 4333217..b780117 100644 --- a/brewpi.py +++ b/brewpi.py @@ -5,6 +5,7 @@ from ws.server.ws_server_multi_user import WsServerMultiUser from components.sensor.tempSensorSim import TempSensorSim, ATemperatureSensor from components.plant.pot import Pot, APlant from components.actor.heater import Heater, AHeater +from components.pid import kalman from ws.message import MsgIo, MessageDispatcher, Value @@ -18,12 +19,13 @@ class ATask: class TempSensorTask(ATask): - def __init__(self, sensor: ATemperatureSensor, interval, msg_handler: MsgIo): + def __init__(self, sensor: ATemperatureSensor, interval, msg_handler: MsgIo, kalman: kalman.Kalman): ATask.__init__(self, interval) self.msg_handler = msg_handler msg_handler.set_recv_handler(self.recv) self.sensor = sensor + self.kalman = kalman async def recv(self, data): print(data) @@ -35,8 +37,13 @@ class TempSensorTask(ATask): print("{}: Started with interval {} s".format(self.msg_handler.get_key(), self.interval)) temp = Value() while True: - temp.set(round(self.sensor.temperature(), 1)) + Z = self.kalman.process_measurement((self.sensor.temperature(), 0)) + xp = self.kalman.process(Z) + theta_ist = xp[0, 0] + # heatrate_ist = xp[1, 0] * 60 + temp.set(round(theta_ist, 1)) if temp.is_changed(): + print("Sensor temp {}".format(temp.get())) await self.send({'Temp': temp.get()}) await asyncio.sleep(self.interval) @@ -109,42 +116,6 @@ class PotTask(ATask): await asyncio.sleep(self.interval) -class CounterTask(ATask): - def __init__(self, interval, msg_handler: MsgIo): - ATask.__init__(self, interval) - - self.msg_handler = msg_handler - msg_handler.set_recv_handler(self.recv) - - async def recv(self, data): - print(data) - - async def send(self, data): - await self.msg_handler.send(data) - - async def on_process(self): - print("{}: Started with interval {} s".format(self.msg_handler.get_key(), self.interval)) - while True: - for count in range(0, 101): - await self.send(count) - await asyncio.sleep(self.interval) - - -class ColorHandler: - def __init__(self, msg_handler: MsgIo): - self.msg_handler = msg_handler - msg_handler.set_recv_handler(self.recv) - print("{}: Constructed".format(self.msg_handler.get_key())) - - async def recv(self, data): - print(data) - for value in data.values(): - await self.send(value) - - async def send(self, data): - await self.msg_handler.send(data) - - class TaskManager: def __init__(self): self.tasks: ATask = [] @@ -174,13 +145,22 @@ if __name__ == '__main__': dispatcher = MessageDispatcher() server = WsServerMultiUser(listener=dispatcher) taskmgr = TaskManager() - taskmgr.add(CounterTask(1.0, dispatcher.msgio_get("a"))) - taskmgr.add(CounterTask(0.5, dispatcher.msgio_get("b"))) - taskmgr.add(CounterTask(0.1, dispatcher.msgio_get("c"))) + + # Kalman Filter + kalman_params = { + "dt" : 1.0, + "var_P" : 0, + "var_Q" : 0.000001, + "var_R" : 1, + "var_Z" : 0.0 + } + kalman = kalman.Kalman(kalman_params) # Sensor sensor = TempSensorSim() - taskmgr.add(TempSensorTask(sensor, 1.0, dispatcher.msgio_get("Sensor"))) + taskmgr.add(TempSensorTask(sensor, 1.0, dispatcher.msgio_get("Sensor"), kalman)) + + # Plant pot_params = { "dt" : 0.1, "theta_amb" : 15, @@ -190,8 +170,6 @@ if __name__ == '__main__': "Td" : 12, "kn" : 0.2 } - # {"Heater": {"Activate": 1, "Power": 1000}} - # Plant pot = Pot(pot_params) pot.connect_theta_changed(sensor.set_fake_temp) taskmgr.add(PotTask(pot, 0.1, dispatcher.msgio_get("Pot"))) @@ -201,8 +179,6 @@ if __name__ == '__main__': heater.connect_power_changed(pot.setPower) taskmgr.add(HeaterTask(heater, 0.1, dispatcher.msgio_get("Heater"))) - ColorHandler(dispatcher.msgio_get("Color")) - h_dispatcher = taskmgr.start() h_server = server.listen("localhost", 8765) asyncio.gather(h_dispatcher, h_server) diff --git a/components/actor/__init__.py b/components/actor/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/components/pid/__init__.py b/components/pid/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/components/pid/kalman.py b/components/pid/kalman.py new file mode 100644 index 0000000..0521012 --- /dev/null +++ b/components/pid/kalman.py @@ -0,0 +1,144 @@ +import numpy as np +from numpy.linalg import inv +from matplotlib.pyplot import plot, figure, subplot, title, xlabel, ylabel, grid, show + + +class Kalman: + def __init__(self, params={}): + dt = params['dt'] + var_P = params['var_P'] + var_Q = params['var_Q'] + var_R = params['var_R'] + var_Z = params['var_Z'] + model = np.matrix([1, dt, 1/2*dt**2]).transpose() + N = len(model)-1 + + P = var_P*np.eye(N) + R = var_R*np.eye(N) + H = np.eye(N) + + A = np.eye(N) + for row in range(0, N): + A[row, row:N] = model.transpose()[0, 0:N-row] + + G = np.matrix(model[N:0:-1]) + Q = G * G.transpose() * var_Q + + self.P = P + self.Q = Q + self.R = R + self.N = N + self.A = A + self.H = H + self.var_Z = var_Z + + X = np.matrix([0, 1.0]).transpose() + Xp = np.matrix([0, 0]).transpose() + self.Xp = Xp + self.X = X + + np.set_printoptions(precision=3) + + @staticmethod + def print(p, d): + print(p) + print(d) + + def initial(self, X): + self.Xp = np.matrix([X[0], X[1]]).transpose() + + def process_truth(self): + # ---------------------------- + # Process ground truth + self.X = self.A * self.X + + return self.X + + def process_measurement(self, y): + + Y = np.matrix([y[0], y[1]]).transpose() + # ---------------------------- + # Take noisy measurement + Z = self.H * Y + self.var_Z * np.random.randn(self.N, 1) + + return Z + + def process(self, Z): + + # ---------------------------- + # State estimate + self.Xp = self.A * self.Xp + + # ---------------------------- + # Measurement prediction + Zp = self.H * self.Xp + + # ---------------------------- + # Measurement residual + V = Z - Zp + + # ---------------------------- + # State prediction covariance + self.P = self.A * self.P * self.A.transpose() + self.Q + + # ---------------------------- + # Measurement prediction covariance + S = self.H * self.P * self.H.transpose() + self.R + + # ---------------------------- + # Kalman gain + K = self.P * self.H.transpose() * inv(S) + + # ---------------------------- + # Update state estimate + self.Xp = self.Xp + K * V + + # ---------------------------- + # Updated state covariance + self.P = self.P - K * S * K + + return self.Xp + +# Main +if __name__ == '__main__': + dt =1 + + params = { + 'dt' : dt, + 'var_P' : 1, + 'var_Q' : 0, + 'var_R' : 1, + 'var_Z' : 0 + } + k = Kalman(params) + + _x1 = np.empty(0) + _y1 = np.empty(0) + _x2 = np.empty(0) + _y2 = np.empty(0) + + N = int(100/dt) + seqn = range(0, N) + + for n in seqn: + X = k.process_truth() + Z = k.process_measurement((X[0,0], 0)) + #Kalman.print("Z:", Z) + Xp = k.process(Z) + + _x1 = np.append(_x1, Z[0]) + _x2 = np.append(_x2, Z[1]) + _y1 = np.append(_y1, Xp[0]) + _y2 = np.append(_y2, Xp[1]) + + figure(1) + subplot(2, 1, 1) + plot(seqn, _x1, 'bx', seqn, _y1, '-r', linewidth=1) + grid(True) + subplot(2, 1, 2) + plot(seqn, _x2, 'bx', seqn, _y2, '-r', linewidth=1) + grid(True) + show() + + print("End of program") + diff --git a/components/pid/pid.py b/components/pid/pid.py new file mode 100644 index 0000000..fb1fc6f --- /dev/null +++ b/components/pid/pid.py @@ -0,0 +1,48 @@ + + +class Pid: + def __init__(self): + # Integrator + self.yi = 0 + + # Differentiator + self.xd = 0 + + # Auto windup + self.y_min = -1.0 + self.y_max = 1.0 + + # Output + self.y = 0 + + @staticmethod + def params(kp, ki, kd, rho): + p = dict(kp=kp, ki=ki, kd=kd, rho=rho) + return p + + def reset(self): + self.yi = 0 + self.xd = 0 + + def process(self, dt, params, err): + kp = params['kp'] + ki = params['ki'] + kd = params['kd'] + rho = params['rho'] + + yi = rho*self.yi + ki*dt * err + yd = err - self.xd + + _yp = kp * err + _yi = yi + _yd = kd/dt * yd + + y = _yp + _yi + _yd + + self.y = max(self.y_min, min(self.y_max, y)) + + self.yi = yi + self.xd = err + + def get_y(self): + return self.y diff --git a/components/pid/temp_controller.py b/components/pid/temp_controller.py index e69de29..c604d50 100644 --- a/components/pid/temp_controller.py +++ b/components/pid/temp_controller.py @@ -0,0 +1,41 @@ +from components.pid import Pid +from components.pid import Kalman + + +class TempController: + def __init__(self, params): + self.dt = params['dt'] + self.pid_hold = Pid() + self.pid_rate = Pid() + self.theta_ist = 0 + self.theta_soll = 0 + self.heatrate_ist = 0 + self.heatrate_soll = 0 + self.params = params + + def set_theta_ist(self, value): + self.theta_ist = value + + def set_heatrate_ist(self, value): + self.heatrate_ist = value + + def set_theta_soll(self, value): + self.theta_soll = value + + def set_heatrate_soll(self, value): + self.heatrate_soll = value + + def process(self): + theta_err = self.theta_soll - self.theta_ist + heatrate_err = self.heatrate_soll - self.heatrate_ist + + self.pid_hold.process(self.dt, self.params['Hold']['Pid'], theta_err) + self.pid_rate.process(self.dt, self.params['Heat']['Pid'], heatrate_err) + + def get_power_hold(self): + power_hold = 200 + self.params['P_max'] * self.pid_hold.get_y() + return max(self.params['P_min'], min(self.params['P_max'], power_hold)) + + def get_power_heat(self): + power_heat = 1500 + self.params['P_max'] * self.pid_rate.get_y() + return max(self.params['P_min'], min(self.params['P_max'], power_heat)) diff --git a/components/plant/__init__.py b/components/plant/__init__.py new file mode 100644 index 0000000..e69de29