- fixed Kalman

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)