- refactored

components/pid/kalman.py

@@ -17,7 +17,8 @@ class Kalman:
 
 		# Sensor Noise Covariance Matrix
 		self.R = var_R*np.eye(N)
-		H = np.eye(N)
+
+		self.H = np.eye(N)
 
 		self.A = np.eye(N)
 		for row in range(0, N):
@@ -28,14 +29,10 @@ class Kalman:
 		# Process Noise Covariance Matrix
 		self.Q = G * G.transpose() * var_Q
 
-		self.N = N
-		self.H = H
-
 		# State Matrix
-		self.X = np.matrix([0, 1.0]).transpose()
+		self.X = np.matrix([0, 0]).transpose()
 
-		# Predicted State Matrix
-		self.Xp = np.matrix([0, 0]).transpose()
+		self.N = N
 
 		np.set_printoptions(precision=3)
 
@@ -45,59 +42,41 @@ class Kalman:
 		print(d)
 
 	def initial(self, X):
-		self.Xp = np.matrix([X[0], X[1]]).transpose()
-
-	def process_truth(self):
-		# ----------------------------
-		# Process ground truth
-		self.X = self.A * self.X
-
-		return self.X
+		self.X = np.matrix([X[0], X[1]]).transpose()
 
 	def process_measurement(self, y, var_Z):
-
-		Y = np.matrix([y[0], y[1]]).transpose()
 		# ----------------------------
 		# Take noisy measurement
-		Z = self.H * Y + var_Z * np.random.randn(self.N, 1)
+		Y = self.H * np.matrix([y[0], y[1]]).transpose() + var_Z * np.random.randn(self.N, 1)
 
-		return Z
+		return Y
 
-	def process(self, Z):
+	def process(self, Y):
 
 		# ----------------------------
-		# State estimate
-		self.Xp = self.A * self.Xp
+		# Predict State estimate
+		X = self.A * self.X
 
 		# ----------------------------
-		# State	prediction covariance
-		self.P = self.A * self.P * self.A.transpose() + self.Q
+		# Predict State	covariance
+		P = self.A * self.P * self.A.transpose() + self.Q
 
 		# ----------------------------
 		# Measurement prediction covariance
-		S = self.H * self.P * self.H.transpose() + self.R
+		S = self.H * P * self.H.transpose() + self.R
 
 		# ----------------------------
 		# Kalman gain
-		K = self.P * self.H.transpose() * inv(S)
-		print("Kalman gain = {}".format(K))
-
-		# ----------------------------
-		# Measurement prediction
-		Zp = self.H * self.Xp
-
-		# ----------------------------
-		# Measurement residual
-		V = Z - Zp
+		K = P * self.H.transpose() * inv(S)
 
 		# Update state estimate
-		self.Xp = self.Xp + K * V
+		self.X = X + K*(Y - self.H * X)
 
 		# ----------------------------
 		# Updated state covariance
-		self.P = self.P - K * S * K
-
-		return self.Xp
+		I = np.eye(self.N)
+		self.P = (I - K * self.H) * P
+		return self.X
 
 
 # Main
@@ -121,8 +100,8 @@ if __name__ == '__main__':
 	seqn = range(0, N)
 
 	_seqn = range(0, 2*N)
+	X = np.array([1, 0])
 	for n in seqn:
-		X = (1, 0)
 		Z = k.process_measurement(X, 0.1)
 		# Kalman.print("Z:", Z)
 		Xp = k.process(Z)
@@ -133,8 +112,8 @@ if __name__ == '__main__':
 		_y2 = np.append(_y2, Xp[1])
 
 	for n in seqn:
-		X = k.process_truth()
-		Z = k.process_measurement((X[0,0], 0), 0.1)
+		X[0] = X[0] + 1
+		Z = k.process_measurement(X, 0.1)
 		# Kalman.print("Z:", Z)
 		Xp = k.process(Z)