-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathpipe2.py
executable file
·30 lines (22 loc) · 977 Bytes
/
pipe2.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
#!/bin/python3
import matplotlib.pyplot as plt
import numpy as np
xEst = np.zeros((2,1))
xTrue = np.zeros((2,1))
while True:
x_Est, y_Est, x_True, y_True = map(float, input().split())
hxEst = np.array([[x_Est],[y_Est]])
xEst = np.hstack((xEst, hxEst))
hxTrue = np.array([[x_True],[y_True]])
xTrue = np.hstack((xTrue, hxTrue))
plt.cla()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
#plotting actual state (represented by blue line)
plt.plot(xTrue[0, :], xTrue[1, :], "-b")
#plotting estimated state (represented by red line)
plt.plot(xEst[0,:], xEst[1,:], "-r")
plt.axis("equal")
plt.grid(True)
plt.pause(0.001)