def main():
print(__file__ + " start!!")
time = 0.0
# RFID positions [x, y]
RFID = np.array([[10.0, 0.0], [10.0, 10.0], [0.0, 15.0], [-5.0, 20.0]])
# State Vector [x y yaw v]'
xEst = np.matrix(np.zeros((4, 1)))
xTrue = np.matrix(np.zeros((4, 1)))
PEst = np.eye(4)
px = np.matrix(np.zeros((4, NP))) # Particle store
pw = np.matrix(np.zeros((1, NP))) + 1.0 / NP # Particle weight
xDR = np.matrix(np.zeros((4, 1))) # Dead reckoning
# history
hxEst = xEst
hxTrue = xTrue
hxDR = xTrue
while SIM_TIME >= time:
time += DT
u = calc_input()
xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)
xEst, PEst, px, pw = pf_localization(px, pw, xEst, PEst, z, ud)
# store data history
hxEst = np.hstack((hxEst, xEst))
hxDR = np.hstack((hxDR, xDR))
hxTrue = np.hstack((hxTrue, xTrue))
if show_animation:
plt.cla()
for i in range(len(z[:, 0])):
plt.plot([xTrue[0, 0], z[i, 1]], [xTrue[1, 0], z[i, 2]], "-k")
plt.plot(RFID[:, 0], RFID[:, 1], "*k")
plt.plot(px[0, :], px[1, :], ".r")
plt.plot(
np.array(hxTrue[0, :]).flatten(),
np.array(hxTrue[1, :]).flatten(), "-b")
plt.plot(
np.array(hxDR[0, :]).flatten(),
np.array(hxDR[1, :]).flatten(), "-k")
plt.plot(
np.array(hxEst[0, :]).flatten(),
np.array(hxEst[1, :]).flatten(), "-r")
plot_covariance_ellipse(xEst, PEst)
plt.axis("equal")
plt.grid(True)
plt.pause(1)
matplotrecorder.save_frame()
def main():
print(__file__ + " start!!")
nx = 4 # State Vector [x y yaw v]'
xEst = np.matrix(np.zeros((nx, 1)))
xTrue = np.matrix(np.zeros((nx, 1)))
PEst = np.eye(nx)
xDR = np.matrix(np.zeros((nx, 1))) # Dead reckoning
wm, wc, gamma = setup_ukf(nx)
# history
hxEst = xEst
hxTrue = xTrue
hxDR = xTrue
hz = np.zeros((1, 2))
time = 0.0
while SIM_TIME >= time:
time += DT
u = calc_input()
xTrue, z, xDR, ud = observation(xTrue, xDR, u)
xEst, PEst = ukf_estimation(xEst, PEst, z, ud, wm, wc, gamma)
# store data history
hxEst = np.hstack((hxEst, xEst))
hxDR = np.hstack((hxDR, xDR))
hxTrue = np.hstack((hxTrue, xTrue))
hz = np.vstack((hz, z))
if show_animation:
plt.cla()
plt.plot(hz[:, 0], hz[:, 1], ".g")
plt.plot(
np.array(hxTrue[0, :]).flatten(),
np.array(hxTrue[1, :]).flatten(), "-b")
plt.plot(
np.array(hxDR[0, :]).flatten(),
np.array(hxDR[1, :]).flatten(), "-k")
plt.plot(
np.array(hxEst[0, :]).flatten(),
np.array(hxEst[1, :]).flatten(), "-r")
plot_covariance_ellipse(xEst, PEst)
plt.axis("equal")
plt.grid(True)
plt.pause(0.001)
matplotrecorder.save_frame() # save each frame
def render(self, cars, non_rl_cars, ax, save_frame=True):
ax.clear()
ax.imshow(self.img)
for i, car in enumerate(cars):
x, y, angle, _ = car.state
self.draw_car(ax, x, y, angle, center='text', text=str(i))
for i, car in enumerate(non_rl_cars):
x, y, angle, _ = car.state
classname = type(car).__name__
if classname == 'ManualCar':
self.draw_car(ax, x, y, angle, center='text', text=f'M{i}', front_color='blue')
# Draw goal point
plt.plot(*car.goal_state.get_goal_pos(), 'bo', markersize=10)
plt.text(*car.goal_state.get_goal_pos(), f'M{i}', color='white', fontsize=6, ha='center', va='center')
# Draw velocity information
plt.text(*self.user_text_point, f'Step size:\nv: {car.v_curr}\ndphi: {car.dphi_curr}', fontsize=6, ha='left', va='top')
if classname == 'RandomCar':
self.draw_car(ax, x, y, angle, center='text', text=f'R{i}', front_color='red')
ax.axis('off')
plt.subplots_adjust(left=0, right=1, top=1, bottom=0)
if save_frame: matplotrecorder.save_frame()
# (5, 5, 1),
# (3, 6, 2),
# (3, 8, 2),
# (3, 10, 2),
# (7, 5, 2),
# (9, 5, 2)
# ] # [x,y,size(radius)]
obstacleList = [(5, 5, 1), (4, 6, 1), (4, 8, 1), (4, 10, 1), (6, 5, 1),
(7, 5, 1), (8, 6, 1), (8, 8, 1),
(8, 10, 1)] # [x,y,size(radius)]
# Set Initial parameters
start = [0.0, 0.0, math.radians(0.0)]
goal = [6.0, 7.0, math.radians(90.0)]
rrt = RRT(start, goal, randArea=[-2.0, 15.0], obstacleList=obstacleList)
path = rrt.Planning(animation=False)
# Draw final path
rrt.DrawGraph()
plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
plt.grid(True)
plt.pause(0.001)
for i in range(10):
matplotrecorder.save_frame() # save each frame
plt.show()
matplotrecorder.save_movie("animation.gif", 0.1)
def main():
parser = argparse.ArgumentParser()
parser = argparse.ArgumentParser()
parser.add_argument('--record', type=int, default=0)
args = parser.parse_args()
is_recorded = args.record
train_dataset = SimpleDataset(total_num=250,
is_confused=True,
x=3,
y=5,
seed=1)
test_dataset = SimpleDataset(total_num=100,
is_confused=False,
x=3,
y=5,
seed=2)
model_PA = PassiveAggressive()
model_PA_one = PassiveAggressiveOne(0.05)
fig = plt.figure(figsize=(20, 4))
gs = gridspec.GridSpec(1, 10)
fig_left = fig.add_subplot(gs[0, :3])
fig_right = fig.add_subplot(gs[0, 4:])
fig_left.set_xlim([
train_dataset.dataset.x1.min() - 0.1,
train_dataset.dataset.x1.max() + 0.1
])
fig_left.set_ylim([
train_dataset.dataset.x2.min() - 0.1,
train_dataset.dataset.x2.max() + 0.1
])
fig_left.set_title("Input Data & Trained Boundary", fontsize=15)
fig_left.tick_params(labelsize=10)
fig_right.set_xlim([0, len(train_dataset.y)])
fig_right.set_ylim([0, 1])
fig_right.set_title("Test Accuracy", fontsize=15)
fig_right.tick_params(labelsize=10)
fig_right.set_xlabel("Number of training data", fontsize=12)
fig_right.set_ylabel("Accuracy", fontsize=12)
line_x = np.array(range(-10, 10, 1))
line_y = line_x * 0
line_PA, = fig_left.plot(line_x, line_y, c="#2980b9", label="PA")
line_PA_one, = fig_left.plot(line_x, line_y, c="#e74c3c", label="PA-1")
fig_left.legend(handles=[line_PA, line_PA_one], fontsize=12)
fig_right.legend(handles=[line_PA, line_PA_one], fontsize=12)
valid_result_sample = []
accuracies_PA = []
accuracies_PA_one = []
imgs = []
for i in range(len(train_dataset.y)):
fig_left.scatter(x=train_dataset.dataset.x1[i],
y=train_dataset.dataset.x2[i],
c=cm.cool(train_dataset.dataset.label[i]),
alpha=0.5)
model_PA.fit(train_dataset.feature_vec[i], train_dataset.y[i])
model_PA_one.fit(train_dataset.feature_vec[i], train_dataset.y[i])
accuracies_PA.append(test_dataset.valid_training_result(model_PA))
accuracies_PA_one.append(
test_dataset.valid_training_result(model_PA_one))
a, b, c = model_PA.w
line_y = (a * line_x + c) / (-b)
line_PA.set_data(line_x, line_y)
a, b, c = model_PA_one.w
line_y = (a * line_x + c) / (-b)
line_PA_one.set_data(line_x, line_y)
fig_right.plot(accuracies_PA, c="#2980b9", label="PA")
fig_right.plot(accuracies_PA_one, c="#e74c3c", label="PA-1")
plt.pause(0.005)
if is_recorded == 1:
matplotrecorder.save_frame()
if is_recorded == 1:
matplotrecorder.save_movie("results.mp4", 0.005)
