- fixed Kalman

brewpi.py

@@ -35,15 +35,16 @@ class TempSensorTask(ATask):
 
 	async def on_process(self):
 		print("{}: Started with interval {} s".format(self.msg_handler.get_key(), self.interval))
-		temp = Value()
+		temp = Value(self.sensor.temperature())
+		self.kalman.initial((temp.get(), 0))
 		while True:
-			Z = self.kalman.process_measurement((self.sensor.temperature(), 0))
+			Z = self.kalman.process_measurement((self.sensor.temperature(), 0), 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()))
+				print("Sensor temp {}".format(self.sensor.temperature()))
 				await self.send({'Temp': temp.get()})
 			await asyncio.sleep(self.interval)
 
@@ -149,10 +150,9 @@ if __name__ == '__main__':
 		# Kalman Filter
 		kalman_params = {
 			"dt"				: 1.0,
-			"var_P"				: 0,
-			"var_Q"				: 0.000001,
-			"var_R"				: 1,
-			"var_Z"				: 0.0
+			"var_P"				: 0.1,
+			"var_Q"				: 0.001,
+			"var_R"				: 0.1
 		}
 		kalman = kalman.Kalman(kalman_params)
 
@@ -162,7 +162,7 @@ if __name__ == '__main__':
 
 		# Plant
 		pot_params = {
-			"dt"			: 0.1,
+			"dt"			: 1.0,
 			"theta_amb"		: 15,
 			"C"				: 4190,
 			"M"				: 20,
@@ -172,12 +172,12 @@ if __name__ == '__main__':
 		}
 		pot = Pot(pot_params)
 		pot.connect_theta_changed(sensor.set_fake_temp)
-		taskmgr.add(PotTask(pot, 0.1, dispatcher.msgio_get("Pot")))
+		taskmgr.add(PotTask(pot, 1.0, dispatcher.msgio_get("Pot")))
 
 		# Heater
 		heater = Heater()
 		heater.connect_power_changed(pot.setPower)
-		taskmgr.add(HeaterTask(heater, 0.1, dispatcher.msgio_get("Heater")))
+		taskmgr.add(HeaterTask(heater, 1.0, dispatcher.msgio_get("Heater")))
 
 		h_dispatcher = taskmgr.start()
 		h_server = server.listen("localhost", 8765)

components/pid/kalman.py

@@ -9,7 +9,6 @@ class Kalman:
 		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
 
@@ -30,7 +29,6 @@ class Kalman:
 		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()
@@ -54,12 +52,12 @@ class Kalman:
 
 		return self.X
 
-	def process_measurement(self, y):
+	def process_measurement(self, y, var_Z):
 
 		Y = np.matrix([y[0], y[1]]).transpose()
 		# ----------------------------
 		# Take noisy measurement
-		Z = self.H * Y + self.var_Z * np.random.randn(self.N, 1)
+		Z = self.H * Y + var_Z * np.random.randn(self.N, 1)
 
 		return Z
 
@@ -99,16 +97,16 @@ class Kalman:
 
 		return self.Xp
 
+
 # Main
 if __name__ == '__main__':
-	dt =1
+	dt = 1
 
 	params = {
 		'dt'	: dt,
-		'var_P'	: 1,
-		'var_Q'	: 0,
-		'var_R'	: 1,
-		'var_Z'	: 0
+		'var_P'	: 0.1,
+		'var_Q'	: 0.001,
+		'var_R'	: 0.1
 	}
 	k = Kalman(params)
 
@@ -121,9 +119,9 @@ if __name__ == '__main__':
 	seqn = range(0, N)
 
 	for n in seqn:
-		X = k.process_truth()
-		Z = k.process_measurement((X[0,0], 0))
-		#Kalman.print("Z:", Z)
+		X = ()
+		Z = k.process_measurement((1, 0), 0.01)
+		# Kalman.print("Z:", Z)
 		Xp = k.process(Z)
 
 		_x1 = np.append(_x1, Z[0])

components/plant/pot.py

@@ -5,7 +5,6 @@ from components.aplant import APlant
 
 class Pot(APlant):
 	def __init__(self, params):
-		print(json.dumps({'Pot': params}, indent=4, sort_keys=True))
 		self.dt = params['dt']
 		self.alpha = 1.0
 

components/sensor/tempSensorSim.py

@@ -17,5 +17,5 @@ class TempSensorSim(ATemperatureSensor):
 
 	def temperature(self):
 		self.count += 1
-		return self.temp + 0.0*math.sin(2*math.pi*self.count*self.freq)
+		return self.temp + 0.1*math.sin(2*math.pi*self.count*self.freq)