- refactored Kalman

git-svn-id: http://moon:8086/svn/projects/HendiControl@210 fda53097-d464-4ada-af97-ba876c37ca34

Control/brewpi/kalman.py

@@ -32,7 +32,7 @@ class Kalman():
 		self.var_Z = var_Z
 
 		X = np.matrix([0, 1.0]).transpose()
-		Xp = np.matrix([0, -1.0]).transpose()
+		Xp = np.matrix([0, 0]).transpose()
 		self.Xp = Xp
 		self.X = X
 
@@ -43,25 +43,30 @@ class Kalman():
 		print(p)
 		print(d)
 
-	def process(self):
+	def initial(self, X):
-
+		self.Xp = np.matrix([X[0], X[1]]).transpose()
-		_x1 = np.empty(0)
-		_y1 = np.empty(0)
-		_x2 = np.empty(0)
-		_y2 = np.empty(0)
-		for n in range(0, 100):
-
-			# ----------------------------
-			# State estimate
-			self.Xp = self.A * self.Xp
 
+	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 * self.X + self.var_Z * np.random.randn(self.N, 1)
+		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
@@ -91,35 +96,46 @@ class Kalman():
 		# Updated state covariance
 		self.P = self.P - K * S * K
 
-			# ----------------------------
+		return self.Xp
-			# Plot vars
-			_x1 = np.append(_x1, Z[0])
-			_x2 = np.append(_x2, Z[1])
-			_y1 = np.append(_y1, self.Xp[0])
-			_y2 = np.append(_y2, self.Xp[1])
 
-		n = range(0,100)
-		figure(1)
-		subplot(2, 1, 1)
-		plot(n, _x1, 'bx', n, _y1, '-r', linewidth=1)
-		grid(True)
-		subplot(2, 1, 2)
-		plot(n, _x2, 'bx', n, _y2, '-r', linewidth=1)
-		grid(True)
-		show()
 # Main
 if __name__ == '__main__':
 
 	params = {
 		'dt'	: 1,
 		'var_P'	: 1,
-		'var_Q'	: .001,
+		'var_Q'	: 0,
 		'var_R'	: 1,
-		'var_Z'	: 1
+		'var_Z'	: 0
 	}
 	k = Kalman(params)
 
-	k.process()
+	_x1 = np.empty(0)
+	_y1 = np.empty(0)
+	_x2 = np.empty(0)
+	_y2 = np.empty(0)
+
+	seqn = range(0, 100)
+
+	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")