mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-02-11 08:36:18 -05:00
add # pragma: no cover
This commit is contained in:
@@ -656,7 +656,7 @@ def main():
|
|||||||
print(f'Converged after {it + 1} iterations.')
|
print(f'Converged after {it + 1} iterations.')
|
||||||
break
|
break
|
||||||
|
|
||||||
if show_animation:
|
if show_animation: # pragma: no cover
|
||||||
plot_animation(X, U)
|
plot_animation(X, U)
|
||||||
|
|
||||||
print("done!!")
|
print("done!!")
|
||||||
|
|||||||
@@ -122,7 +122,7 @@ def main():
|
|||||||
ex, ey, er, error = circle_fitting(x, y)
|
ex, ey, er, error = circle_fitting(x, y)
|
||||||
print("Error:", error)
|
print("Error:", error)
|
||||||
|
|
||||||
if show_animation:
|
if show_animation: # pragma: no cover
|
||||||
plt.cla()
|
plt.cla()
|
||||||
plt.axis("equal")
|
plt.axis("equal")
|
||||||
plt.plot(0.0, 0.0, "*r")
|
plt.plot(0.0, 0.0, "*r")
|
||||||
|
|||||||
@@ -71,7 +71,7 @@ def main():
|
|||||||
gmap, minx, maxx, miny, maxy = generate_gaussian_grid_map(
|
gmap, minx, maxx, miny, maxy = generate_gaussian_grid_map(
|
||||||
ox, oy, xyreso, STD)
|
ox, oy, xyreso, STD)
|
||||||
|
|
||||||
if show_animation:
|
if show_animation: # pragma: no cover
|
||||||
plt.cla()
|
plt.cla()
|
||||||
draw_heatmap(gmap, minx, maxx, miny, maxy, xyreso)
|
draw_heatmap(gmap, minx, maxx, miny, maxy, xyreso)
|
||||||
plt.plot(ox, oy, "xr")
|
plt.plot(ox, oy, "xr")
|
||||||
|
|||||||
@@ -161,7 +161,7 @@ def main():
|
|||||||
clusters = kmeans_clustering(rx, ry, ncluster)
|
clusters = kmeans_clustering(rx, ry, ncluster)
|
||||||
|
|
||||||
# for animation
|
# for animation
|
||||||
if show_animation:
|
if show_animation: # pragma: no cover
|
||||||
plt.cla()
|
plt.cla()
|
||||||
inds = calc_association(cx, cy, clusters)
|
inds = calc_association(cx, cy, clusters)
|
||||||
for ic in inds:
|
for ic in inds:
|
||||||
|
|||||||
@@ -121,7 +121,8 @@ def main():
|
|||||||
oy = (np.random.rand(4) - 0.5) * 10.0
|
oy = (np.random.rand(4) - 0.5) * 10.0
|
||||||
pmap, minx, maxx, miny, maxy, xyreso = generate_ray_casting_grid_map(
|
pmap, minx, maxx, miny, maxy, xyreso = generate_ray_casting_grid_map(
|
||||||
ox, oy, xyreso, yawreso)
|
ox, oy, xyreso, yawreso)
|
||||||
if show_animation:
|
|
||||||
|
if show_animation: # pragma: no cover
|
||||||
plt.cla()
|
plt.cla()
|
||||||
draw_heatmap(pmap, minx, maxx, miny, maxy, xyreso)
|
draw_heatmap(pmap, minx, maxx, miny, maxy, xyreso)
|
||||||
plt.plot(ox, oy, "xr")
|
plt.plot(ox, oy, "xr")
|
||||||
|
|||||||
@@ -68,7 +68,7 @@ def a_star_planning(sx, sy, gx, gy, ox, oy, reso, rr):
|
|||||||
current = openset[c_id]
|
current = openset[c_id]
|
||||||
|
|
||||||
# show graph
|
# show graph
|
||||||
if show_animation:
|
if show_animation: # pragma: no cover
|
||||||
plt.plot(current.x * reso, current.y * reso, "xc")
|
plt.plot(current.x * reso, current.y * reso, "xc")
|
||||||
if len(closedset.keys()) % 10 == 0:
|
if len(closedset.keys()) % 10 == 0:
|
||||||
plt.pause(0.001)
|
plt.pause(0.001)
|
||||||
@@ -214,7 +214,7 @@ def main():
|
|||||||
ox.append(40.0)
|
ox.append(40.0)
|
||||||
oy.append(60.0 - i)
|
oy.append(60.0 - i)
|
||||||
|
|
||||||
if show_animation:
|
if show_animation: # pragma: no cover
|
||||||
plt.plot(ox, oy, ".k")
|
plt.plot(ox, oy, ".k")
|
||||||
plt.plot(sx, sy, "xr")
|
plt.plot(sx, sy, "xr")
|
||||||
plt.plot(gx, gy, "xb")
|
plt.plot(gx, gy, "xb")
|
||||||
@@ -223,7 +223,7 @@ def main():
|
|||||||
|
|
||||||
rx, ry = a_star_planning(sx, sy, gx, gy, ox, oy, grid_size, robot_size)
|
rx, ry = a_star_planning(sx, sy, gx, gy, ox, oy, grid_size, robot_size)
|
||||||
|
|
||||||
if show_animation:
|
if show_animation: # pragma: no cover
|
||||||
plt.plot(rx, ry, "-r")
|
plt.plot(rx, ry, "-r")
|
||||||
plt.show()
|
plt.show()
|
||||||
|
|
||||||
|
|||||||
@@ -313,7 +313,7 @@ def test1(max_vel=0.5):
|
|||||||
for j, t in enumerate(times):
|
for j, t in enumerate(times):
|
||||||
state[:, j] = traj.calc_traj_point(t)
|
state[:, j] = traj.calc_traj_point(t)
|
||||||
|
|
||||||
if show_animation:
|
if show_animation: # pragma: no cover
|
||||||
# plot the path
|
# plot the path
|
||||||
plt.plot(state[0, :], state[1, :])
|
plt.plot(state[0, :], state[1, :])
|
||||||
plt.pause(1.0)
|
plt.pause(1.0)
|
||||||
|
|||||||
@@ -343,7 +343,7 @@ def main():
|
|||||||
print("Goal")
|
print("Goal")
|
||||||
break
|
break
|
||||||
|
|
||||||
if show_animation:
|
if show_animation: # pragma: no cover
|
||||||
plt.cla()
|
plt.cla()
|
||||||
plt.plot(tx, ty)
|
plt.plot(tx, ty)
|
||||||
plt.plot(ob[:, 0], ob[:, 1], "xk")
|
plt.plot(ob[:, 0], ob[:, 1], "xk")
|
||||||
@@ -356,7 +356,7 @@ def main():
|
|||||||
plt.pause(0.0001)
|
plt.pause(0.0001)
|
||||||
|
|
||||||
print("Finish")
|
print("Finish")
|
||||||
if show_animation:
|
if show_animation: # pragma: no cover
|
||||||
plt.grid(True)
|
plt.grid(True)
|
||||||
plt.pause(0.0001)
|
plt.pause(0.0001)
|
||||||
plt.show()
|
plt.show()
|
||||||
|
|||||||
@@ -304,7 +304,7 @@ def main():
|
|||||||
path = rrt.planning(animation=show_animation)
|
path = rrt.planning(animation=show_animation)
|
||||||
|
|
||||||
# Draw final path
|
# Draw final path
|
||||||
if show_animation:
|
if show_animation: # pragma: no cover
|
||||||
rrt.draw_graph()
|
rrt.draw_graph()
|
||||||
plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
|
plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
|
||||||
plt.grid(True)
|
plt.grid(True)
|
||||||
|
|||||||
@@ -123,7 +123,7 @@ def optimize_trajectory(target, k0, p):
|
|||||||
p += alpha * np.array(dp)
|
p += alpha * np.array(dp)
|
||||||
# print(p.T)
|
# print(p.T)
|
||||||
|
|
||||||
if show_animation:
|
if show_animation: # pragma: no cover
|
||||||
show_trajectory(target, xc, yc)
|
show_trajectory(target, xc, yc)
|
||||||
else:
|
else:
|
||||||
xc, yc, yawc, p = None, None, None, None
|
xc, yc, yawc, p = None, None, None, None
|
||||||
|
|||||||
@@ -393,7 +393,7 @@ def test():
|
|||||||
px, py, pyaw, mode, clen = reeds_shepp_path_planning(
|
px, py, pyaw, mode, clen = reeds_shepp_path_planning(
|
||||||
start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature, step_size)
|
start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature, step_size)
|
||||||
|
|
||||||
if show_animation:
|
if show_animation: # pragma: no cover
|
||||||
plt.cla()
|
plt.cla()
|
||||||
plt.plot(px, py, label="final course " + str(mode))
|
plt.plot(px, py, label="final course " + str(mode))
|
||||||
|
|
||||||
@@ -430,7 +430,7 @@ def main():
|
|||||||
px, py, pyaw, mode, clen = reeds_shepp_path_planning(
|
px, py, pyaw, mode, clen = reeds_shepp_path_planning(
|
||||||
start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature, step_size)
|
start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature, step_size)
|
||||||
|
|
||||||
if show_animation:
|
if show_animation: # pragma: no cover
|
||||||
plt.cla()
|
plt.cla()
|
||||||
plt.plot(px, py, label="final course " + str(mode))
|
plt.plot(px, py, label="final course " + str(mode))
|
||||||
|
|
||||||
|
|||||||
@@ -78,7 +78,7 @@ def VRM_planning(sx, sy, gx, gy, ox, oy, rr):
|
|||||||
obkdtree = KDTree(np.vstack((ox, oy)).T)
|
obkdtree = KDTree(np.vstack((ox, oy)).T)
|
||||||
|
|
||||||
sample_x, sample_y = sample_points(sx, sy, gx, gy, rr, ox, oy, obkdtree)
|
sample_x, sample_y = sample_points(sx, sy, gx, gy, rr, ox, oy, obkdtree)
|
||||||
if show_animation:
|
if show_animation: # pragma: no cover
|
||||||
plt.plot(sample_x, sample_y, ".b")
|
plt.plot(sample_x, sample_y, ".b")
|
||||||
|
|
||||||
road_map = generate_roadmap(sample_x, sample_y, rr, obkdtree)
|
road_map = generate_roadmap(sample_x, sample_y, rr, obkdtree)
|
||||||
@@ -183,7 +183,7 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y):
|
|||||||
current = openset[c_id]
|
current = openset[c_id]
|
||||||
|
|
||||||
# show graph
|
# show graph
|
||||||
if show_animation and len(closedset.keys()) % 2 == 0:
|
if show_animation and len(closedset.keys()) % 2 == 0: # pragma: no cover
|
||||||
plt.plot(current.x, current.y, "xg")
|
plt.plot(current.x, current.y, "xg")
|
||||||
plt.pause(0.001)
|
plt.pause(0.001)
|
||||||
|
|
||||||
@@ -287,7 +287,7 @@ def main():
|
|||||||
ox.append(40.0)
|
ox.append(40.0)
|
||||||
oy.append(60.0 - i)
|
oy.append(60.0 - i)
|
||||||
|
|
||||||
if show_animation:
|
if show_animation: # pragma: no cover
|
||||||
plt.plot(ox, oy, ".k")
|
plt.plot(ox, oy, ".k")
|
||||||
plt.plot(sx, sy, "^r")
|
plt.plot(sx, sy, "^r")
|
||||||
plt.plot(gx, gy, "^c")
|
plt.plot(gx, gy, "^c")
|
||||||
@@ -298,7 +298,7 @@ def main():
|
|||||||
|
|
||||||
assert rx, 'Cannot found path'
|
assert rx, 'Cannot found path'
|
||||||
|
|
||||||
if show_animation:
|
if show_animation: # pragma: no cover
|
||||||
plt.plot(rx, ry, "-r")
|
plt.plot(rx, ry, "-r")
|
||||||
plt.show()
|
plt.show()
|
||||||
|
|
||||||
|
|||||||
@@ -584,7 +584,7 @@ def main():
|
|||||||
# update state
|
# update state
|
||||||
plant_system.update_state(u_1s[0], u_2s[0])
|
plant_system.update_state(u_1s[0], u_2s[0])
|
||||||
|
|
||||||
if show_animation:
|
if show_animation: # pragma: no cover
|
||||||
animation(plant_system, controller, dt)
|
animation(plant_system, controller, dt)
|
||||||
plot_figures(plant_system, controller, iteration_num, dt)
|
plot_figures(plant_system, controller, iteration_num, dt)
|
||||||
|
|
||||||
|
|||||||
@@ -1,3 +1,8 @@
|
|||||||
|
import cubic_spline_planner
|
||||||
|
import scipy.linalg as la
|
||||||
|
import matplotlib.pyplot as plt
|
||||||
|
import math
|
||||||
|
import numpy as np
|
||||||
"""
|
"""
|
||||||
|
|
||||||
Path tracking simulation with LQR speed and steering control
|
Path tracking simulation with LQR speed and steering control
|
||||||
@@ -8,11 +13,6 @@ author Atsushi Sakai (@Atsushi_twi)
|
|||||||
import sys
|
import sys
|
||||||
sys.path.append("../../PathPlanning/CubicSpline/")
|
sys.path.append("../../PathPlanning/CubicSpline/")
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
import math
|
|
||||||
import matplotlib.pyplot as plt
|
|
||||||
import scipy.linalg as la
|
|
||||||
import cubic_spline_planner
|
|
||||||
|
|
||||||
# LQR parameter
|
# LQR parameter
|
||||||
Q = np.eye(5)
|
Q = np.eye(5)
|
||||||
@@ -206,8 +206,8 @@ def closed_loop_prediction(cx, cy, cyaw, ck, speed_profile, goal):
|
|||||||
plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
|
plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
|
||||||
plt.axis("equal")
|
plt.axis("equal")
|
||||||
plt.grid(True)
|
plt.grid(True)
|
||||||
plt.title("speed[km/h]:" + str(round(state.v * 3.6, 2)) +
|
plt.title("speed[km/h]:" + str(round(state.v * 3.6, 2))
|
||||||
",target index:" + str(target_ind))
|
+ ",target index:" + str(target_ind))
|
||||||
plt.pause(0.0001)
|
plt.pause(0.0001)
|
||||||
|
|
||||||
return t, x, y, yaw, v
|
return t, x, y, yaw, v
|
||||||
@@ -257,7 +257,7 @@ def main():
|
|||||||
|
|
||||||
t, x, y, yaw, v = closed_loop_prediction(cx, cy, cyaw, ck, sp, goal)
|
t, x, y, yaw, v = closed_loop_prediction(cx, cy, cyaw, ck, sp, goal)
|
||||||
|
|
||||||
if show_animation:
|
if show_animation: # pragma: no cover
|
||||||
plt.close()
|
plt.close()
|
||||||
plt.subplots(1)
|
plt.subplots(1)
|
||||||
plt.plot(ax, ay, "xb", label="waypoints")
|
plt.plot(ax, ay, "xb", label="waypoints")
|
||||||
|
|||||||
@@ -255,7 +255,7 @@ def main():
|
|||||||
|
|
||||||
t, x, y, yaw, v = closed_loop_prediction(cx, cy, cyaw, ck, sp, goal)
|
t, x, y, yaw, v = closed_loop_prediction(cx, cy, cyaw, ck, sp, goal)
|
||||||
|
|
||||||
if show_animation:
|
if show_animation: # pragma: no cover
|
||||||
plt.close()
|
plt.close()
|
||||||
plt.subplots(1)
|
plt.subplots(1)
|
||||||
plt.plot(ax, ay, "xb", label="input")
|
plt.plot(ax, ay, "xb", label="input")
|
||||||
|
|||||||
@@ -427,7 +427,7 @@ def do_simulation(cx, cy, cyaw, ck, sp, dl, initial_state):
|
|||||||
print("Goal")
|
print("Goal")
|
||||||
break
|
break
|
||||||
|
|
||||||
if show_animation:
|
if show_animation: # pragma: no cover
|
||||||
plt.cla()
|
plt.cla()
|
||||||
if ox is not None:
|
if ox is not None:
|
||||||
plt.plot(ox, oy, "xr", label="MPC")
|
plt.plot(ox, oy, "xr", label="MPC")
|
||||||
@@ -563,7 +563,7 @@ def main():
|
|||||||
t, x, y, yaw, v, d, a = do_simulation(
|
t, x, y, yaw, v, d, a = do_simulation(
|
||||||
cx, cy, cyaw, ck, sp, dl, initial_state)
|
cx, cy, cyaw, ck, sp, dl, initial_state)
|
||||||
|
|
||||||
if show_animation:
|
if show_animation: # pragma: no cover
|
||||||
plt.close("all")
|
plt.close("all")
|
||||||
plt.subplots()
|
plt.subplots()
|
||||||
plt.plot(cx, cy, "-r", label="spline")
|
plt.plot(cx, cy, "-r", label="spline")
|
||||||
@@ -596,7 +596,7 @@ def main2():
|
|||||||
t, x, y, yaw, v, d, a = do_simulation(
|
t, x, y, yaw, v, d, a = do_simulation(
|
||||||
cx, cy, cyaw, ck, sp, dl, initial_state)
|
cx, cy, cyaw, ck, sp, dl, initial_state)
|
||||||
|
|
||||||
if show_animation:
|
if show_animation: # pragma: no cover
|
||||||
plt.close("all")
|
plt.close("all")
|
||||||
plt.subplots()
|
plt.subplots()
|
||||||
plt.plot(cx, cy, "-r", label="spline")
|
plt.plot(cx, cy, "-r", label="spline")
|
||||||
|
|||||||
@@ -67,7 +67,7 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal):
|
|||||||
x = x + v * np.cos(theta) * dt
|
x = x + v * np.cos(theta) * dt
|
||||||
y = y + v * np.sin(theta) * dt
|
y = y + v * np.sin(theta) * dt
|
||||||
|
|
||||||
if show_animation:
|
if show_animation: # pragma: no cover
|
||||||
plt.cla()
|
plt.cla()
|
||||||
plt.arrow(x_start, y_start, np.cos(theta_start),
|
plt.arrow(x_start, y_start, np.cos(theta_start),
|
||||||
np.sin(theta_start), color='r', width=0.1)
|
np.sin(theta_start), color='r', width=0.1)
|
||||||
|
|||||||
@@ -126,7 +126,7 @@ def main():
|
|||||||
v.append(state.v)
|
v.append(state.v)
|
||||||
t.append(time)
|
t.append(time)
|
||||||
|
|
||||||
if show_animation:
|
if show_animation: # pragma: no cover
|
||||||
plt.cla()
|
plt.cla()
|
||||||
plt.plot(cx, cy, ".r", label="course")
|
plt.plot(cx, cy, ".r", label="course")
|
||||||
plt.plot(x, y, "-b", label="trajectory")
|
plt.plot(x, y, "-b", label="trajectory")
|
||||||
@@ -139,7 +139,7 @@ def main():
|
|||||||
# Test
|
# Test
|
||||||
assert lastIndex >= target_ind, "Cannot goal"
|
assert lastIndex >= target_ind, "Cannot goal"
|
||||||
|
|
||||||
if show_animation:
|
if show_animation: # pragma: no cover
|
||||||
plt.cla()
|
plt.cla()
|
||||||
plt.plot(cx, cy, ".r", label="course")
|
plt.plot(cx, cy, ".r", label="course")
|
||||||
plt.plot(x, y, "-b", label="trajectory")
|
plt.plot(x, y, "-b", label="trajectory")
|
||||||
|
|||||||
@@ -1,3 +1,6 @@
|
|||||||
|
import cubic_spline_planner
|
||||||
|
import matplotlib.pyplot as plt
|
||||||
|
import math
|
||||||
"""
|
"""
|
||||||
|
|
||||||
Path tracking simulation with rear wheel feedback steering control and PID speed control.
|
Path tracking simulation with rear wheel feedback steering control and PID speed control.
|
||||||
@@ -8,10 +11,6 @@ author: Atsushi Sakai(@Atsushi_twi)
|
|||||||
import sys
|
import sys
|
||||||
sys.path.append("../../PathPlanning/CubicSpline/")
|
sys.path.append("../../PathPlanning/CubicSpline/")
|
||||||
|
|
||||||
import math
|
|
||||||
import matplotlib.pyplot as plt
|
|
||||||
import cubic_spline_planner
|
|
||||||
|
|
||||||
|
|
||||||
Kp = 1.0 # speed propotional gain
|
Kp = 1.0 # speed propotional gain
|
||||||
# steering control parameter
|
# steering control parameter
|
||||||
@@ -202,7 +201,7 @@ def main():
|
|||||||
# Test
|
# Test
|
||||||
assert goal_flag, "Cannot goal"
|
assert goal_flag, "Cannot goal"
|
||||||
|
|
||||||
if show_animation:
|
if show_animation: # pragma: no cover
|
||||||
plt.close()
|
plt.close()
|
||||||
plt.subplots(1)
|
plt.subplots(1)
|
||||||
plt.plot(ax, ay, "xb", label="input")
|
plt.plot(ax, ay, "xb", label="input")
|
||||||
|
|||||||
@@ -184,7 +184,7 @@ def main():
|
|||||||
v.append(state.v)
|
v.append(state.v)
|
||||||
t.append(time)
|
t.append(time)
|
||||||
|
|
||||||
if show_animation:
|
if show_animation: # pragma: no cover
|
||||||
plt.cla()
|
plt.cla()
|
||||||
plt.plot(cx, cy, ".r", label="course")
|
plt.plot(cx, cy, ".r", label="course")
|
||||||
plt.plot(x, y, "-b", label="trajectory")
|
plt.plot(x, y, "-b", label="trajectory")
|
||||||
@@ -197,7 +197,7 @@ def main():
|
|||||||
# Test
|
# Test
|
||||||
assert last_idx >= target_idx, "Cannot reach goal"
|
assert last_idx >= target_idx, "Cannot reach goal"
|
||||||
|
|
||||||
if show_animation:
|
if show_animation: # pragma: no cover
|
||||||
plt.plot(cx, cy, ".r", label="course")
|
plt.plot(cx, cy, ".r", label="course")
|
||||||
plt.plot(x, y, "-b", label="trajectory")
|
plt.plot(x, y, "-b", label="trajectory")
|
||||||
plt.legend()
|
plt.legend()
|
||||||
|
|||||||
@@ -97,12 +97,12 @@ def observation(xTrue, xd, u, RFID):
|
|||||||
def motion_model(x, u):
|
def motion_model(x, u):
|
||||||
|
|
||||||
F = np.array([[1.0, 0, 0],
|
F = np.array([[1.0, 0, 0],
|
||||||
[0, 1.0, 0],
|
[0, 1.0, 0],
|
||||||
[0, 0, 1.0]])
|
[0, 0, 1.0]])
|
||||||
|
|
||||||
B = np.array([[DT * math.cos(x[2, 0]), 0],
|
B = np.array([[DT * math.cos(x[2, 0]), 0],
|
||||||
[DT * math.sin(x[2, 0]), 0],
|
[DT * math.sin(x[2, 0]), 0],
|
||||||
[0.0, DT]])
|
[0.0, DT]])
|
||||||
|
|
||||||
x = (F @ x) + (B @ u)
|
x = (F @ x) + (B @ u)
|
||||||
return x
|
return x
|
||||||
@@ -119,8 +119,8 @@ def jacob_motion(x, u):
|
|||||||
(STATE_SIZE, LM_SIZE * calc_n_LM(x)))))
|
(STATE_SIZE, LM_SIZE * calc_n_LM(x)))))
|
||||||
|
|
||||||
jF = np.array([[0.0, 0.0, -DT * u[0] * math.sin(x[2, 0])],
|
jF = np.array([[0.0, 0.0, -DT * u[0] * math.sin(x[2, 0])],
|
||||||
[0.0, 0.0, DT * u[0] * math.cos(x[2, 0])],
|
[0.0, 0.0, DT * u[0] * math.cos(x[2, 0])],
|
||||||
[0.0, 0.0, 0.0]])
|
[0.0, 0.0, 0.0]])
|
||||||
|
|
||||||
G = np.eye(STATE_SIZE) + Fx.T * jF * Fx
|
G = np.eye(STATE_SIZE) + Fx.T * jF * Fx
|
||||||
|
|
||||||
@@ -170,7 +170,7 @@ def calc_innovation(lm, xEst, PEst, z, LMid):
|
|||||||
delta = lm - xEst[0:2]
|
delta = lm - xEst[0:2]
|
||||||
q = (delta.T @ delta)[0, 0]
|
q = (delta.T @ delta)[0, 0]
|
||||||
#zangle = math.atan2(delta[1], delta[0]) - xEst[2]
|
#zangle = math.atan2(delta[1], delta[0]) - xEst[2]
|
||||||
zangle = math.atan2(delta[1,0], delta[0,0]) - xEst[2]
|
zangle = math.atan2(delta[1, 0], delta[0, 0]) - xEst[2]
|
||||||
zp = np.array([[math.sqrt(q), pi_2_pi(zangle)]])
|
zp = np.array([[math.sqrt(q), pi_2_pi(zangle)]])
|
||||||
y = (z - zp).T
|
y = (z - zp).T
|
||||||
y[1] = pi_2_pi(y[1])
|
y[1] = pi_2_pi(y[1])
|
||||||
@@ -183,7 +183,7 @@ def calc_innovation(lm, xEst, PEst, z, LMid):
|
|||||||
def jacobH(q, delta, x, i):
|
def jacobH(q, delta, x, i):
|
||||||
sq = math.sqrt(q)
|
sq = math.sqrt(q)
|
||||||
G = np.array([[-sq * delta[0, 0], - sq * delta[1, 0], 0, sq * delta[0, 0], sq * delta[1, 0]],
|
G = np.array([[-sq * delta[0, 0], - sq * delta[1, 0], 0, sq * delta[0, 0], sq * delta[1, 0]],
|
||||||
[delta[1, 0], - delta[0, 0], - 1.0, - delta[1, 0], delta[0, 0]]])
|
[delta[1, 0], - delta[0, 0], - 1.0, - delta[1, 0], delta[0, 0]]])
|
||||||
|
|
||||||
G = G / q
|
G = G / q
|
||||||
nLM = calc_n_LM(x)
|
nLM = calc_n_LM(x)
|
||||||
@@ -235,13 +235,12 @@ def main():
|
|||||||
|
|
||||||
x_state = xEst[0:STATE_SIZE]
|
x_state = xEst[0:STATE_SIZE]
|
||||||
|
|
||||||
|
|
||||||
# store data history
|
# store data history
|
||||||
hxEst = np.hstack((hxEst, x_state))
|
hxEst = np.hstack((hxEst, x_state))
|
||||||
hxDR = np.hstack((hxDR, xDR))
|
hxDR = np.hstack((hxDR, xDR))
|
||||||
hxTrue = np.hstack((hxTrue, xTrue))
|
hxTrue = np.hstack((hxTrue, xTrue))
|
||||||
|
|
||||||
if show_animation:
|
if show_animation: # pragma: no cover
|
||||||
plt.cla()
|
plt.cla()
|
||||||
|
|
||||||
plt.plot(RFID[:, 0], RFID[:, 1], "*k")
|
plt.plot(RFID[:, 0], RFID[:, 1], "*k")
|
||||||
@@ -263,6 +262,5 @@ def main():
|
|||||||
plt.pause(0.001)
|
plt.pause(0.001)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
if __name__ == '__main__':
|
||||||
main()
|
main()
|
||||||
@@ -373,7 +373,7 @@ def main():
|
|||||||
hxDR = np.hstack((hxDR, xDR))
|
hxDR = np.hstack((hxDR, xDR))
|
||||||
hxTrue = np.hstack((hxTrue, xTrue))
|
hxTrue = np.hstack((hxTrue, xTrue))
|
||||||
|
|
||||||
if show_animation:
|
if show_animation: # pragma: no cover
|
||||||
plt.cla()
|
plt.cla()
|
||||||
plt.plot(RFID[:, 0], RFID[:, 1], "*k")
|
plt.plot(RFID[:, 0], RFID[:, 1], "*k")
|
||||||
|
|
||||||
|
|||||||
@@ -402,7 +402,7 @@ def main():
|
|||||||
hxDR = np.hstack((hxDR, xDR))
|
hxDR = np.hstack((hxDR, xDR))
|
||||||
hxTrue = np.hstack((hxTrue, xTrue))
|
hxTrue = np.hstack((hxTrue, xTrue))
|
||||||
|
|
||||||
if show_animation:
|
if show_animation: # pragma: no cover
|
||||||
plt.cla()
|
plt.cla()
|
||||||
plt.plot(RFID[:, 0], RFID[:, 1], "*k")
|
plt.plot(RFID[:, 0], RFID[:, 1], "*k")
|
||||||
|
|
||||||
|
|||||||
@@ -295,14 +295,13 @@ def main():
|
|||||||
|
|
||||||
xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)
|
xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)
|
||||||
|
|
||||||
|
|
||||||
hz.append(z)
|
hz.append(z)
|
||||||
|
|
||||||
if dtime >= show_graph_dtime:
|
if dtime >= show_graph_dtime:
|
||||||
x_opt = graph_based_slam(hxDR, hz)
|
x_opt = graph_based_slam(hxDR, hz)
|
||||||
dtime = 0.0
|
dtime = 0.0
|
||||||
|
|
||||||
if show_animation:
|
if show_animation: # pragma: no cover
|
||||||
plt.cla()
|
plt.cla()
|
||||||
|
|
||||||
plt.plot(RFID[:, 0], RFID[:, 1], "*k")
|
plt.plot(RFID[:, 0], RFID[:, 1], "*k")
|
||||||
|
|||||||
@@ -33,7 +33,7 @@ def ICP_matching(ppoints, cpoints):
|
|||||||
while dError >= EPS:
|
while dError >= EPS:
|
||||||
count += 1
|
count += 1
|
||||||
|
|
||||||
if show_animation:
|
if show_animation: # pragma: no cover
|
||||||
plt.cla()
|
plt.cla()
|
||||||
plt.plot(ppoints[0, :], ppoints[1, :], ".r")
|
plt.plot(ppoints[0, :], ppoints[1, :], ".r")
|
||||||
plt.plot(cpoints[0, :], cpoints[1, :], ".b")
|
plt.plot(cpoints[0, :], cpoints[1, :], ".b")
|
||||||
|
|||||||
Reference in New Issue
Block a user