- added Pid

- added Temp Controller
- added Kalman

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")))
+		# Kalman Filter
-		taskmgr.add(CounterTask(0.1, dispatcher.msgio_get("c")))
+		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)

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")
+

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

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))