- fixed Kalman

components/pid/kalman.py

@@ -118,9 +118,21 @@ if __name__ == '__main__':
 	N = int(100/dt)
 	seqn = range(0, N)
 
+	_seqn = range(0, 2*N)
 	for n in seqn:
-		X = ()
+		X = (1, 0)
-		Z = k.process_measurement((1, 0), 0.01)
+		Z = k.process_measurement(X, 0.01)
+		# 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])
+
+	for n in seqn:
+		X = k.process_truth()
+		Z = k.process_measurement((X[0,0], 0), 0.01)
 		# Kalman.print("Z:", Z)
 		Xp = k.process(Z)
 
@@ -131,10 +143,10 @@ if __name__ == '__main__':
 
 	figure(1)
 	subplot(2, 1, 1)
-	plot(seqn, _x1, 'bx', seqn, _y1, '-r', linewidth=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)
+	plot(_seqn, _x2, 'bx', _seqn, _y2, '-r', linewidth=1)
 	grid(True)
 	show()