components/pid/temp_controller_smith.py

@@ -1,82 +1,61 @@
 from components.plant.pot import Pot
-from components.pid import Kalman
 from components.pid.temp_controller_base import TempControllerBase
-from components.pid.tc_constants import *
+from components.pid.tc_constants import States
 
 
 class TempController(TempControllerBase):
 	def __init__(self, dt, params, model_params):
 		TempControllerBase.__init__(self, dt, params)
-		self.kalman_model = Kalman(dt, params['Kalman'])
+		self.dt = dt
-		self.kalman_model_delay = Kalman(dt, params['Kalman'])
+		self.last_theta_ist = 20
-		self.kalman_plant = Kalman(dt, params['Kalman'])
+		self.heatrate_ist = 0
-		self.model = Pot(dt, model_params)
+
+		# Fast model: same plant model but with zero transport delay, used
+		# to predict the current temperature without the dead time.
+		self.model = Pot(dt, {**model_params, 'Td': 0})
+		# Delayed model: keeps the plant's assumed transport delay, so it
+		# can be compared like-for-like against the real (delayed) measurement.
+		self.model_delay = Pot(dt, model_params)
 
 		self.theta_ist_plant = 0
-		self.dtheta_ist_plant = 0
 		self.theta_ist_model = 0
-		self.dtheta_ist_model = 0
 		self.theta_ist_model_delay = 0
-		self.dtheta_ist_model_delay = 0
 
 	def set_model_power(self, power):
 		self.model.set_power(power)
-
+		self.model_delay.set_power(power)
-	def init_kalman(self, value):
-		self.kalman_model.initial((value, 0))
-		self.kalman_model_delay.initial((value, 0))
-		self.kalman_plant.initial((value, 0))
 
 	def on_state_entered(self, state):
 		if state == States.HEAT:
 			self.model.initial(self.theta_ist)
-			self.kalman_model.initial((self.theta_ist, 0))
+			self.model_delay.initial(self.theta_ist)
-			self.kalman_model_delay.initial((self.theta_ist, 0))
 
 	def post_pid(self):
 		self.model.process()
+		self.model_delay.process()
 
 	def process(self):
-		# Process Kalman of Plant
+		self.theta_ist_plant = self.theta_ist_set
-		Z_plant = self.kalman_plant.process_measurement((self.theta_ist_set, 0), 0.0)
+		self.theta_ist_model = self.model.get_temperature()
-		xp_plant = self.kalman_plant.process(Z_plant)
+		self.theta_ist_model_delay = self.model_delay.get_temperature()
-		theta_ist_plant = xp_plant[0, 0]
-		heatrate_ist_plant = xp_plant[1, 0] * 60
 
-		# Process Kalman of Model
+		# Smith predictor: use the fast model's prediction, corrected by the
-		k_model = self.kalman_model.process_measurement((self.model.get_temperature_intermediate(), 0), 0.0)
+		# mismatch between the real (delayed) plant and the delayed model,
-		xp_model = self.kalman_model.process(k_model)
+		# so the dead time drops out of the feedback path.
-		theta_ist_model = xp_model[0, 0]
+		self.theta_ist = self.theta_ist_model + (self.theta_ist_plant - self.theta_ist_model_delay)
-		heatrate_ist_model = xp_model[1, 0] * 60
 
-		# Process Kalman of delayed Model
+		heatrate = (self.theta_ist - self.last_theta_ist)/self.dt*60
-		k_model_delay = self.kalman_model_delay.process_measurement((self.model.get_temperature(), 0), 0.0)
+		self.heatrate_ist = heatrate
-		xp_model_delay = self.kalman_model_delay.process(k_model_delay)
+		self.last_theta_ist = self.theta_ist
-		theta_ist_model_delay = xp_model_delay[0, 0]
-		dtheta_ist_model_delay = xp_model_delay[1, 0] * 60
-
-		self.theta_ist_plant = theta_ist_plant
-		self.dtheta_ist_plant = heatrate_ist_plant
-		self.theta_ist_model = theta_ist_model
-		self.dtheta_ist_model = heatrate_ist_model
-		self.theta_ist_model_delay = theta_ist_model_delay
-		self.dtheta_ist_model_delay = dtheta_ist_model_delay
-		self.theta_ist = theta_ist_plant
-		self.heatrate_ist = heatrate_ist_plant
 
 		# Compensate for max heat rate to reduce overshoot
 		if self.heatrate_soll_set > 0:
 			self.pid_hold.scale(1.0/self.heatrate_soll_set)
 
 		self.heatrate_soll = self.heatrate_soll_set * self.pid_hold.get_y()
+		theta_err = self.theta_soll_set - self.theta_ist
+		heatrate_err = self.heatrate_soll - self.heatrate_ist
 
-		if 1:
-			theta_err = self.theta_soll_set - (theta_ist_plant - theta_ist_model_delay + theta_ist_model)
-		else:
-			theta_err = self.theta_soll_set - theta_ist_plant
-
-		heatrate_err = self.heatrate_soll - (heatrate_ist_plant - dtheta_ist_model_delay + heatrate_ist_model)
 		diff = self.theta_soll_set - self.theta_ist
-
 		self.process_fsm(diff)
 		self.process_pid(theta_err, heatrate_err)

scripts/demos/pid/demo_temp_controller_smith.py

@@ -2,38 +2,62 @@ import numpy as np
 from matplotlib.pyplot import plot, figure, subplot, grid, show, legend
 from components.plant.pot import Pot
 from components.pid.temp_controller_smith import TempController
-from components.pid.tc_constants import Test
 
 
 if __name__ == '__main__':
-	dt = 1.0
+	ctrl_params = {
+		"Hold": {
+			"kp": 0.4,
+			"ki": 0.0,
+			"kd": 0.0,
+			"kt": 0.0
+		},
+		"Heat": {
+			"kp": 0.08,
+			"ki": 0.008,
+			"kd": 0.0,
+			"kt": 1.5
+		}
+	}
 
+	plant_params = {
+		"theta"		: 20,
+		"C"			: 4190,
+		"M"			: 20,
+		"L"			: 0.2,
+		"Td"		: 30,
+		"kn"		: 0.2,
+		"gain"		: 1.0
+	}
+
+	dt = 1.0
+	k_noise = 0.001
+
+	temp_ist = 0
 	temp_soll = 20
-	ctrl = TempController(dt, Test.tc_ctrl_params, Test.tc_model_params)
+	heatrate_soll = 1.25
-	plant = Pot(dt, Test.tc_pot_params)
+	ctrl = TempController(dt, ctrl_params, plant_params)
+	plant = Pot(dt, plant_params)
 	_y = np.empty(0)
 	_fb = np.empty(0)
 	_t = np.empty(0)
 	_temp_soll = np.empty(0)
-	_temp_ist_kalman = np.empty(0)
+	_heatrate_ist = np.empty(0)
-	_heatrate_ist_kalman = np.empty(0)
+	_temp_ist = np.empty(0)
-	_temp_ist_kalman_plant = np.empty(0)
+	_temp_ist_model = np.empty(0)
-	_heatrate_ist_kalman_plant = np.empty(0)
+	_temp_ist_model_delay = np.empty(0)
-	_temp_ist_kalman_model = np.empty(0)
-	_heatrate_ist_kalman_model = np.empty(0)
 	a = 0.5
 	fb = 0
 	rho = 0.02
 	temps = [{'Temp': 20, 'Duration': 1000}, {'Temp': 40, 'Duration': 1000}, {'Temp': 50, 'Duration': 1000}, {'Temp': 60, 'Duration': 1000}, {'Temp': 70, 'Duration': 1000}, {'Temp': 80, 'Duration': 1000}, {'Temp': 78, 'Duration': 1000}]
 
 	t = 0
-
 	for temp in temps:
 		temp_soll = temp['Temp']
 		hold_counter = temp['Duration']
 		hold = False
 		ctrl.set_theta_soll(temp_soll)
-		ctrl.set_heatrate_soll(1.0)
+		ctrl.set_heatrate_soll(heatrate_soll)
 
 		while True:
 			if hold:
@@ -41,30 +65,32 @@ if __name__ == '__main__':
 					break
 				hold_counter -= 1
 			plant.process()
-			temp_ist = plant.get_temperature()
+			temp_ist = plant.get_temperature() + k_noise * np.random.randn()
 			ctrl.set_theta_ist(temp_ist)
 			ctrl.process()
 
 			y = 3500*ctrl.get_power()
 			power = max(0, y)
 			plant.set_power(power)
+			ctrl.set_model_power(power)
 			fb = plant.get_power()
 			if abs(temp_ist - temp_soll) < 0.1:
 				hold = True
 
+			temp_ist -= rho
+			_temp_ist = np.append(_temp_ist, temp_ist)
+			_temp_soll = np.append(_temp_soll, temp_soll)
 			_y = np.append(_y, y)
 			_fb = np.append(_fb, fb)
 			_t = np.append(_t, t)
-			_temp_soll = np.append(_temp_soll, temp_soll)
+			_heatrate_ist = np.append(_heatrate_ist, max(-1, min(3, ctrl.heatrate_ist)))
-			_temp_ist_kalman_plant = np.append(_temp_ist_kalman_plant, ctrl.theta_ist_plant)
+			_temp_ist_model = np.append(_temp_ist_model, ctrl.theta_ist_model)
-			_heatrate_ist_kalman_plant = np.append(_heatrate_ist_kalman_plant, max(-1, min(3, ctrl.dtheta_ist_plant)))
+			_temp_ist_model_delay = np.append(_temp_ist_model_delay, ctrl.theta_ist_model_delay)
-			_temp_ist_kalman_model = np.append(_temp_ist_kalman_model, ctrl.theta_ist_model_delay)
-			_heatrate_ist_kalman_model = np.append(_heatrate_ist_kalman_model, max(-1, min(3, ctrl.dtheta_ist_model_delay)))
 			t += 1
 
 	figure(1)
 	subplot(3, 1, 1)
-	plot(_t, _temp_ist_kalman_plant, _t, _temp_soll, 'r-', linewidth=1)
+	plot(_t, _temp_ist, _t, _temp_soll, 'r-', linewidth=1)
 	legend(["ist", "soll"])
 	grid(True)
 	subplot(3, 1, 2)
@@ -72,18 +98,13 @@ if __name__ == '__main__':
 	legend(["y", "pot"])
 	grid(True)
 	subplot(3, 1, 3)
-	plot(_t, _heatrate_ist_kalman_plant, '-b', linewidth=1)
+	plot(_t, _heatrate_ist, '-b', linewidth=1)
 	legend(["heatrate"])
 	grid(True)
 
 	figure(2)
-	subplot(2, 1, 1)
+	plot(_t, _temp_ist, '-b', _t, _temp_ist_model, '-g', _t, _temp_ist_model_delay, '-r', linewidth=1)
-	plot(_t, _temp_ist_kalman_plant, '-b', _t, _temp_ist_kalman_model, 'r-', linewidth=1)
+	legend(["plant", "model (fast)", "model (delayed)"])
-	legend(["plant", "model"])
-	grid(True)
-	subplot(2, 1, 2)
-	plot(_t, _heatrate_ist_kalman_plant, '-b', _t, _heatrate_ist_kalman_model, '-r', linewidth=1)
-	legend(["plant", "model"])
 	grid(True)
 	show()