- refactored

components/pid/kalman.py

@@ -12,28 +12,30 @@ class Kalman:
 		model = np.matrix([1, dt, 1/2*dt**2]).transpose()
 		N = len(model)-1
 
-		P = var_P*np.eye(N)
-		R = var_R*np.eye(N)
+		# Process Covariance Matrix
+		self.P = var_P*np.eye(N)
+
+		# Sensor Noise Covariance Matrix
+		self.R = var_R*np.eye(N)
 		H = np.eye(N)
 
-		A = np.eye(N)
+		self.A = np.eye(N)
 		for row in range(0, N):
-			A[row, row:N] = model.transpose()[0, 0:N-row]
+			self.A[row, row:N] = model.transpose()[0, 0:N-row]
 
 		G = np.matrix(model[N:0:-1])
-		Q = G * G.transpose() * var_Q
 
-		self.P = P
-		self.Q = Q
-		self.R = R
+		# Process Noise Covariance Matrix
+		self.Q = G * G.transpose() * var_Q
+
 		self.N = N
-		self.A = A
 		self.H = H
 
-		X = np.matrix([0, 1.0]).transpose()
-		Xp = np.matrix([0, 0]).transpose()
-		self.Xp = Xp
-		self.X = X
+		# State Matrix
+		self.X = np.matrix([0, 1.0]).transpose()
+
+		# Predicted State Matrix
+		self.Xp = np.matrix([0, 0]).transpose()
 
 		np.set_printoptions(precision=3)
 
@@ -67,14 +69,6 @@ class Kalman:
 		# State estimate
 		self.Xp = self.A * self.Xp
 
-		# ----------------------------
-		# Measurement prediction
-		Zp = self.H * self.Xp
-
-		# ----------------------------
-		# Measurement residual
-		V = Z - Zp
-
 		# ----------------------------
 		# State	prediction covariance
 		self.P = self.A * self.P * self.A.transpose() + self.Q
@@ -86,8 +80,16 @@ class Kalman:
 		# ----------------------------
 		# 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
+
 		# Update state estimate
 		self.Xp = self.Xp + K * V