From 9dbd92a379ab0377f968c8ea344bd4f3c3cc147a Mon Sep 17 00:00:00 2001
From: Jens Ahrensfeld <jens@jayfield.org>
Date: Wed, 3 Apr 2019 05:17:40 +0000
Subject: [PATCH] - added Kalman to brewpi controller

git-svn-id: http://moon:8086/svn/projects/HendiControl@211 fda53097-d464-4ada-af97-ba876c37ca34
---
 Control/brewpi/brewpi.py     |  6 +--
 Control/brewpi/controller.py | 75 ++++++++++++++++++++++++++----------
 2 files changed, 57 insertions(+), 24 deletions(-)

diff --git a/Control/brewpi/brewpi.py b/Control/brewpi/brewpi.py
index 237dff2..25e4a80 100755
--- a/Control/brewpi/brewpi.py
+++ b/Control/brewpi/brewpi.py
@@ -13,12 +13,12 @@ def results_plot(self):
 
 	figure(1)
 	subplot(4, 1, 1)
-	plot(self.time_v, self.theta_v, 'b-', linewidth=1)
+	plot(self.time_v, self.theta_v, 'b-', self.time_v, self.theta_k_v, 'r-', linewidth=1)
 	title('Temperature')
 	grid(True)
 	ylabel('°C')
 	subplot(4, 1, 2)
-	plot(self.time_v, self.power_v, 'r-', linewidth=1)
+	plot(self.time_v, self.power_v, 'b-', linewidth=1)
 	title('Power')
 	grid(True)
 	ylabel('W')
@@ -28,7 +28,7 @@ def results_plot(self):
 	grid(True)
 	ylabel('°C')
 	subplot(4, 1, 4)
-	plot(self.time_v, self.heatrate_v, 'r-', linewidth=1)
+	plot(self.time_v, self.heatrate_v, 'b-', self.time_v, self.heatrate_k_v, 'r-', linewidth=1)
 	title('Heatrate')
 	grid(True)
 	ylabel('°C/min')
diff --git a/Control/brewpi/controller.py b/Control/brewpi/controller.py
index 96368a2..ca66f34 100644
--- a/Control/brewpi/controller.py
+++ b/Control/brewpi/controller.py
@@ -5,6 +5,7 @@ from pid import Pid
 from utils import Smoother, Stable
 import time
 import threading
+from kalman import Kalman
 
 ovenStates = Enum('ovenStates', 'NOP HEAT HOLD')
 controllerStates = Enum('controllerStates', 'ACQU TRACK')
@@ -22,10 +23,12 @@ class Controller():
 		self.params = params
 		self.pid = Pid()
 		self.timer_ist = 0
-		self.theta_ist = 0
-		self.heatrate_ist = 0
+		self.theta = 0
+		self.heatrate = 0
 		self.theta_v = np.empty(0)
+		self.theta_k_v = np.empty(0)
 		self.heatrate_v = np.empty(0)
+		self.heatrate_k_v = np.empty(0)
 		self.time_v = np.empty(0)
 		self.power_v = np.empty(0)
 		self.error_v = np.empty(0)
@@ -40,10 +43,19 @@ class Controller():
 		self.is_restart = False
 		self.receipe = None
 		self.theta_sm = Smoother(1.0)
-		self.heatrate_sm = Smoother(0.5)
+		self.heatrate_sm = Smoother(0.01)
 		self.theta_err_sm = Smoother(1.0)
 		self.heatrate_err_sm = Smoother(1.0)
 
+		kparams = {
+			'dt': 1,
+			'var_P': 0,
+			'var_Q': 0.00001,
+			'var_R': 2,
+			'var_Z': 0
+		}
+		self.kalman = Kalman(kparams)
+
 	def log(self, s):
 		now = time.time()
 		print("{:.2f}: {}".format(now, s))
@@ -52,8 +64,8 @@ class Controller():
 		now = time.time()
 		if (now - self.report_last_time) >= self.report_interval:
 
-			self.log("Temp IST = {:0.1f} °C".format(self.theta_ist))
-			self.log("Heatrate IST = {:0.1f} °C/min.".format(self.heatrate_ist))
+			self.log("Temp IST = {:0.1f} °C".format(self.theta))
+			self.log("Heatrate IST = {:0.1f} °C/min.".format(self.heatrate))
 			self.log("Power = {:0.0f} W".format(self.plant.getPower()))
 			if self.timer_ist > 0:
 				self.log("Rast remaining : {:0.1f} min.".format(self.timer_ist/60))
@@ -123,7 +135,7 @@ class Controller():
 		self.stirrer.setCycleTime(self.stirrCycleTime)
 
 		# Temperature
-		self.theta_ist = self.plant.getTemperature()
+		self.theta = self.plant.getTemperature()
 		theta_soll = rast['temp']
 		heatrate_soll = rast['heatRate']
 		self.log("Target temperature {} °C".format(theta_soll))
@@ -132,10 +144,15 @@ class Controller():
 		self.timer_ist = 0
 		timer_soll = 60*rast['time']
 
+		# Kalman filter initial value
+		self.kalman.initial((self.theta, 0))
+		self.theta_sm.initial(self.theta)
+
 		# ------------------------------
 		# The loop
 		# ------------------------------
 		doLoop = True
+		useKalman = True
 		while(doLoop):
 
 			time_start = time.time()
@@ -148,35 +165,53 @@ class Controller():
 			self.plant.process()
 
 			theta = self.plant.getTemperature()
+
+			# Process Kalman
+			Z = self.kalman.process_measurement((theta, 0))
+			xp = self.kalman.process(Z)
+			theta_ist_k = xp[0, 0]
+			dtheta_ist_k = xp[1, 0]
+			self.theta_k_v = np.append(self.theta_k_v, theta_ist_k)
+			self.heatrate_k_v = np.append(self.heatrate_k_v, dtheta_ist_k * 60 / self.dt)
+
+			# Process conventional
 			theta_ist = self.theta_sm.process(theta)
+			dtheta_ist = self.heatrate_sm.process(theta_ist - self.theta)
+			self.theta_v = np.append(self.theta_v, theta_ist)
+			self.heatrate_v = np.append(self.heatrate_v, dtheta_ist * 60/self.dt)
 
-			heatrate = 60/self.dt * (theta_ist - self.theta_ist)
-			heatrate_ist = self.heatrate_sm.process(heatrate)
+			if useKalman:
+				ctrl_theta_err = theta_soll - theta_ist_k
+				ctrl_heatrate_err = heatrate_soll - dtheta_ist_k * 60/self.dt
+				ctrl_theta = theta_ist_k
+				ctrl_dtheta = dtheta_ist_k
+			else:
+				theta_err = theta_soll - theta_ist
+				dtheta_err = heatrate_soll - dtheta_ist
+				ctrl_theta_err = self.theta_err_sm.process(theta_err)
+				ctrl_heatrate_err = self.heatrate_err_sm.process(60/self.dt * dtheta_err)
+				ctrl_theta = theta_ist
+				ctrl_dtheta = dtheta_ist
 
-			theta_err = theta_soll - theta_ist
-			heatrate_err = heatrate_soll - heatrate_ist
-
-			self.theta_err_sm.process(theta_err)
-			self.heatrate_err_sm.process(heatrate_err)
 
 			if self.ovenState == ovenStates.NOP:
-				if theta_ist < theta_soll:
+				if ctrl_theta < theta_soll:
 					ovenStateNext = ovenStates.HEAT
 
 			pid_err = 0
 			if self.ovenState == ovenStates.HEAT:
-				pid_err = self.heatrate_err_sm.get_y()
+				pid_err = ctrl_heatrate_err
 				pid_params_acqu = self.params['Heat']['Acqu']['Pid']
 				pid_params_track = self.params['Heat']['Track']['Pid']
 				pid_thresh_acqu = 0.2
 				pid_thresh_track = 0.5
-				if (theta_ist + 1.0) >= theta_soll:
+				if (ctrl_theta + 1.0) >= theta_soll:
 					ovenStateNext = ovenStates.HOLD
 					self.log("Set rast timer to {:0.1f} min.".format(timer_soll/60))
 					self.timer_ist = timer_soll
 
 			if self.ovenState == ovenStates.HOLD:
-				pid_err = self.theta_err_sm.get_y()
+				pid_err = ctrl_theta_err
 				pid_params_acqu = self.params['Hold']['Acqu']['Pid']
 				pid_params_track = self.params['Hold']['Track']['Pid']
 				pid_thresh_acqu = 0.2
@@ -202,8 +237,6 @@ class Controller():
 			power = max(self.params['P_min'], min(self.params['P_max'], self.params['P_max']*y))
 			self.plant.setPower(power)
 
-			self.theta_v = np.append(self.theta_v, theta_ist)
-			self.heatrate_v = np.append(self.heatrate_v, heatrate_ist)
 			self.time_v = np.append(self.time_v, self.time/60)
 			self.power_v = np.append(self.power_v, power)
 			self.error_v = np.append(self.error_v, pid_err)
@@ -234,8 +267,8 @@ class Controller():
 				time_to_sleep = 0
 
 			self.time += self.dt
-			self.theta_ist = theta_ist
-			self.heatrate_ist = heatrate_ist
+			self.theta = ctrl_theta
+			self.heatrate = ctrl_dtheta * 60 / self.dt
 
 			if self.timer_ist > 0:
 				self.timer_ist -= self.dt