- 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,83 +43,99 @@ 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)
+	def process_truth(self):
-		_y1 = np.empty(0)
+		# ----------------------------
-		_x2 = np.empty(0)
+		# Process ground truth
-		_y2 = np.empty(0)
+		self.X = self.A * self.X
-		for n in range(0, 100):
 
-			# ----------------------------
+		return self.X
-			# State estimate
-			self.Xp = self.A * self.Xp
 
-			# ----------------------------
+	def process_measurement(self, y):
-			# Process ground truth
-			self.X = self.A * self.X
 
-			# ----------------------------
+		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)
+		# Take noisy measurement
+		Z = self.H * Y + self.var_Z * np.random.randn(self.N, 1)
 
-			# ----------------------------
+		return Z
-			# Measurement prediction
-			Zp = self.H * self.Xp
 
-			# ----------------------------
+	def process(self, Z):
-			# Measurement residual
-			V = Z - Zp
 
-			# ----------------------------
+		# ----------------------------
-			# State	prediction covariance
+		# State estimate
-			self.P = self.A * self.P * self.A.transpose() + self.Q
+		self.Xp = self.A * self.Xp
 
-			# ----------------------------
+		# ----------------------------
-			# Measurement prediction covariance
+		# Measurement prediction
-			S = self.H * self.P * self.H.transpose() + self.R
+		Zp = self.H * self.Xp
 
-			# ----------------------------
+		# ----------------------------
-			# Kalman gain
+		# Measurement residual
-			K = self.P * self.H.transpose() * inv(S)
+		V = Z - Zp
 
-			# ----------------------------
+		# ----------------------------
-			# Update state estimate
+		# State	prediction covariance
-			self.Xp = self.Xp + K * V
+		self.P = self.A * self.P * self.A.transpose() + self.Q
 
-			# ----------------------------
+		# ----------------------------
-			# Updated state covariance
+		# Measurement prediction covariance
-			self.P = self.P - K * S * K
+		S = self.H * self.P * self.H.transpose() + self.R
 
-			# ----------------------------
+		# ----------------------------
-			# Plot vars
+		# Kalman gain
-			_x1 = np.append(_x1, Z[0])
+		K = self.P * self.H.transpose() * inv(S)
-			_x2 = np.append(_x2, Z[1])
+
-			_y1 = np.append(_y1, self.Xp[0])
+		# ----------------------------
-			_y2 = np.append(_y2, self.Xp[1])
+		# Update state estimate
+		self.Xp = self.Xp + K * V
+
+		# ----------------------------
+		# Updated state covariance
+		self.P = self.P - K * S * K
+
+		return self.Xp
 
-		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")