github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-14 06:37:57 -05:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #262 from Gjacquenot/master

Browse Source 
Replaced sqrt(x**2 + y**2) with hypot(x,y)
This commit is contained in:
Atsushi Sakai
2019-12-08 10:13:29 +09:00
committed by GitHub
parent 190a28e0a8 c63a80b0ef
commit b1a1d8e69d
35 changed files with 59 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
AerialNavigation/rocket_powered_landing/rocket_powered_landing.py
View File

@@ -195,8 +195,8 @@ class Rocket_Model_6DoF:
[0, 0, 0],
[0, 0, 0],
[0, 0, 0],
[0, 0, 1.00000000000000],
[0, -1.00000000000000, 0]
[0, 0, 1.0],
[0, -1.0, 0]
])
def euler_to_quat(self, a):

2
ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py
View File

@@ -148,7 +148,7 @@ def jacobian_inverse(link_lengths, joint_angles):
def distance_to_goal(current_pos, goal_pos):
x_diff = goal_pos[0] - current_pos[0]
y_diff = goal_pos[1] - current_pos[1]
return np.array([x_diff, y_diff]).T, np.math.sqrt(x_diff**2 + y_diff**2)
return np.array([x_diff, y_diff]).T, np.hypot(x_diff, y_diff)
def ang_diff(theta1, theta2):

2
ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py
View File

@@ -55,7 +55,7 @@ def two_joint_arm(GOAL_TH=0.0, theta1=0.0, theta2=0.0):
wrist = plot_arm(theta1, theta2, x, y)
# check goal
d2goal = np.math.sqrt((wrist[0] - x)**2 + (wrist[1] - y)**2)
d2goal = np.hypot(wrist[0] - x, wrist[1] - y)
if abs(d2goal) < GOAL_TH and x is not None:
return theta1, theta2

2
Localization/ensemble_kalman_filter/ensemble_kalman_filter.py
View File

@@ -44,7 +44,7 @@ def observation(xTrue, xd, u, RFID):
dx = RFID[i, 0] - xTrue[0, 0]
dy = RFID[i, 1] - xTrue[1, 0]
d = math.sqrt(dx ** 2 + dy ** 2)
d = math.hypot(dx, dy)
angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0])
if d <= MAX_RANGE:
dn = d + np.random.randn() * Q_sim[0, 0] ** 0.5 # add noise

4
Localization/histogram_filter/histogram_filter.py
View File

@@ -68,7 +68,7 @@ def calc_gaussian_observation_pdf(gmap, z, iz, ix, iy, std):
# predicted range
x = ix * gmap.xy_reso + gmap.minx
y = iy * gmap.xy_reso + gmap.miny
d = math.sqrt((x - z[iz, 1]) ** 2 + (y - z[iz, 2]) ** 2)
d = math.hypot(x - z[iz, 1], y - z[iz, 2])
# likelihood
pdf = (1.0 - norm.cdf(abs(d - z[iz, 0]), 0.0, std))
@@ -126,7 +126,7 @@ def observation(xTrue, u, RFID):
dx = xTrue[0, 0] - RFID[i, 0]
dy = xTrue[1, 0] - RFID[i, 1]
d = math.sqrt(dx ** 2 + dy ** 2)
d = math.hypot(dx, dy)
if d <= MAX_RANGE:
# add noise to range observation
dn = d + np.random.randn() * NOISE_RANGE

4
Localization/particle_filter/particle_filter.py
View File

@@ -47,7 +47,7 @@ def observation(xTrue, xd, u, RF_ID):
dx = xTrue[0, 0] - RF_ID[i, 0]
dy = xTrue[1, 0] - RF_ID[i, 1]
d = math.sqrt(dx ** 2 + dy ** 2)
d = math.hypot(dx, dy)
if d <= MAX_RANGE:
dn = d + np.random.randn() * Q_sim[0, 0] ** 0.5 # add noise
zi = np.array([[dn, RF_ID[i, 0], RF_ID[i, 1]]])
@@ -116,7 +116,7 @@ def pf_localization(px, pw, z, u):
for i in range(len(z[:, 0])):
dx = x[0, 0] - z[i, 1]
dy = x[1, 0] - z[i, 2]
pre_z = math.sqrt(dx ** 2 + dy ** 2)
pre_z = math.hypot(dx, dy)
dz = pre_z - z[i, 0]
w = w * gauss_likelihood(dz, math.sqrt(Q[0, 0]))

2
Mapping/gaussian_grid_map/gaussian_grid_map.py
View File

@@ -31,7 +31,7 @@ def generate_gaussian_grid_map(ox, oy, xyreso, std):
# Search minimum distance
mindis = float("inf")
for (iox, ioy) in zip(ox, oy):
d = math.sqrt((iox - x)**2 + (ioy - y)**2)
d = math.hypot(iox - x, ioy - y)
if mindis >= d:
mindis = d

2
Mapping/kmeans_clustering/kmeans_clustering.py
View File

@@ -68,7 +68,7 @@ class Clusters:
dx = [icx - px for icx in self.center_x]
dy = [icy - py for icy in self.center_y]
dist_list = [math.sqrt(idx ** 2 + idy ** 2) for (idx, idy) in zip(dx, dy)]
dist_list = [math.hypot(idx, idy) for (idx, idy) in zip(dx, dy)]
min_dist = min(dist_list)
min_id = dist_list.index(min_dist)
self.labels[ip] = min_id

4
Mapping/raycasting_grid_map/raycasting_grid_map.py
View File

@@ -57,7 +57,7 @@ def precasting(minx, miny, xw, yw, xyreso, yawreso):
px = ix * xyreso + minx
py = iy * xyreso + miny
d = math.sqrt(px**2 + py**2)
d = math.hypot(px, py)
angle = atan_zero_to_twopi(py, px)
angleid = int(math.floor(angle / yawreso))
@@ -85,7 +85,7 @@ def generate_ray_casting_grid_map(ox, oy, xyreso, yawreso):
for (x, y) in zip(ox, oy):
d = math.sqrt(x**2 + y**2)
d = math.hypot(x, y)
angle = atan_zero_to_twopi(y, x)
angleid = int(math.floor(angle / yawreso))

2
Mapping/rectangle_fitting/rectangle_fitting.py
View File

@@ -162,7 +162,7 @@ class LShapeFitting():
C = set()
R = self.R0 + self.Rd * np.linalg.norm([ox[i], oy[i]])
for j, _ in enumerate(ox):
d = np.sqrt((ox[i] - ox[j])**2 + (oy[i] - oy[j])**2)
d = np.hypot(ox[i] - ox[j], oy[i] - oy[j])
if d <= R:
C.add(j)
S.append(C)

4
PathPlanning/AStar/a_star.py
View File

@@ -136,7 +136,7 @@ class AStarPlanner:
@staticmethod
def calc_heuristic(n1, n2):
w = 1.0 # weight of heuristic
d = w * math.sqrt((n1.x - n2.x) ** 2 + (n1.y - n2.y) ** 2)
d = w * math.hypot(n1.x - n2.x, n1.y - n2.y)
return d
def calc_grid_position(self, index, minp):
@@ -199,7 +199,7 @@ class AStarPlanner:
for iy in range(self.ywidth):
y = self.calc_grid_position(iy, self.miny)
for iox, ioy in zip(ox, oy):
d = math.sqrt((iox - x) ** 2 + (ioy - y) ** 2)
d = math.hypot(iox - x, ioy - y)
if d <= self.rr:
self.obmap[ix][iy] = True
break

4
PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py
View File

@@ -180,8 +180,8 @@ class BITStar(object):
cBest = self.g_scores[self.goalId]
# Computing the sampling space
cMin = math.sqrt(pow(self.start[0] - self.goal[0], 2)
+ pow(self.start[1] - self.goal[1], 2)) / 1.5
cMin = math.hypot(self.start[0] - self.goal[0],
self.start[1] - self.goal[1]) / 1.5
xCenter = np.array([[(self.start[0] + self.goal[0]) / 2.0],
[(self.start[1] + self.goal[1]) / 2.0], [0]])
a1 = np.array([[(self.goal[0] - self.start[0]) / cMin],

2
PathPlanning/BezierPath/bezier_path.py
View File

@@ -26,7 +26,7 @@ def calc_4points_bezier_path(sx, sy, syaw, ex, ey, eyaw, offset):
:param offset: (float)
:return: (numpy array, numpy array)
"""
dist = np.sqrt((sx - ex) ** 2 + (sy - ey) ** 2) / offset
dist = np.hypot(sx - ex, sy - ey) / offset
control_points = np.array(
[[sx, sy],
[sx + dist * np.cos(syaw), sy + dist * np.sin(syaw)],

6
PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py
View File

@@ -6,7 +6,6 @@ author: AtsushiSakai(@Atsushi_twi)
"""
import math
import os
import sys
@@ -119,9 +118,8 @@ class ClosedLoopRRTStar(RRTStarReedsShepp):
print("final angle is bad")
find_goal = False
travel = sum([abs(iv) * unicycle_model.dt for iv in v])
origin_travel = sum([math.sqrt(dx ** 2 + dy ** 2)
for (dx, dy) in zip(np.diff(cx), np.diff(cy))])
travel = unicycle_model.dt * sum(np.abs(v))
origin_travel = sum(np.hypot(np.diff(cx), np.diff(cy)))
if (travel / origin_travel) >= self.invalid_travel_ratio:
print("path is too long")

10
PathPlanning/ClosedLoopRRTStar/pure_pursuit.py
View File

@@ -68,17 +68,17 @@ def calc_target_index(state, cx, cy):
dx = [state.x - icx for icx in cx]
dy = [state.y - icy for icy in cy]
d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)]
d = np.hypot(dx, dy)
mindis = min(d)
ind = d.index(mindis)
ind = np.argmin(d)
L = 0.0
while Lf > L and (ind + 1) < len(cx):
dx = cx[ind + 1] - cx[ind]
dy = cy[ind + 1] - cy[ind]
L += math.sqrt(dx ** 2 + dy ** 2)
L += math.hypot(dx, dy)
ind += 1
# print(mindis)
@@ -121,7 +121,7 @@ def closed_loop_prediction(cx, cy, cyaw, speed_profile, goal):
# check goal
dx = state.x - goal[0]
dy = state.y - goal[1]
if math.sqrt(dx ** 2 + dy ** 2) <= goal_dis:
if math.hypot(dx, dy) <= goal_dis:
find_goal = True
break
@@ -161,7 +161,7 @@ def set_stop_point(target_speed, cx, cy, cyaw):
for i in range(len(cx) - 1):
dx = cx[i + 1] - cx[i]
dy = cy[i + 1] - cy[i]
d.append(math.sqrt(dx ** 2.0 + dy ** 2.0))
d.append(math.hypot(dx, dy))
iyaw = cyaw[i]
move_direction = math.atan2(dy, dx)
is_back = abs(move_direction - iyaw) >= math.pi / 2.0

3
PathPlanning/CubicSpline/cubic_spline_planner.py
View File

@@ -140,8 +140,7 @@ class Spline2D:
def __calc_s(self, x, y):
dx = np.diff(x)
dy = np.diff(y)
self.ds = [math.sqrt(idx ** 2 + idy ** 2)
for (idx, idy) in zip(dx, dy)]
self.ds = np.hypot(dx, dy)
s = [0]
s.extend(np.cumsum(self.ds))
return s

4
PathPlanning/Dijkstra/dijkstra.py
View File

@@ -123,7 +123,7 @@ class Dijkstra:
def calc_heuristic(self, n1, n2):
w = 1.0 # weight of heuristic
d = w * math.sqrt((n1.x - n2.x)**2 + (n1.y - n2.y)**2)
d = w * math.hypot(n1.x - n2.x, n1.y - n2.y)
return d
def calc_position(self, index, minp):
@@ -178,7 +178,7 @@ class Dijkstra:
for iy in range(self.ywidth):
y = self.calc_position(iy, self.miny)
for iox, ioy in zip(ox, oy):
d = math.sqrt((iox - x)**2 + (ioy - y)**2)
d = math.hypot(iox - x, ioy - y)
if d <= self.rr:
self.obmap[ix][iy] = True
break

2
PathPlanning/DubinsPath/dubins_path_planning.py
View File

@@ -141,7 +141,7 @@ def dubins_path_planning_from_origin(ex, ey, eyaw, c, D_ANGLE):
# normalize
dx = ex
dy = ey
D = math.sqrt(dx ** 2.0 + dy ** 2.0)
D = math.hypot(dx, dy)
d = D * c
# print(dx, dy, D, d)

4
PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
View File

@@ -164,7 +164,7 @@ def calc_obstacle_cost(trajectory, ob, config):
oy = ob[:, 1]
dx = trajectory[:, 0] - ox[:, None]
dy = trajectory[:, 1] - oy[:, None]
r = np.sqrt(np.square(dx) + np.square(dy))
r = np.hypot(dx, dy)
if config.robot_type == RobotType.rectangle:
yaw = trajectory[:, 2]
@@ -279,7 +279,7 @@ def main(gx=10.0, gy=10.0, robot_type=RobotType.circle):
plt.pause(0.0001)
# check reaching goal
dist_to_goal = math.sqrt((x[0] - goal[0]) ** 2 + (x[1] - goal[1]) ** 2)
dist_to_goal = math.hypot(x[0] - goal[0], x[1] - goal[1])
if dist_to_goal <= config.robot_radius:
print("Goal!!")
break

2
PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py
View File

@@ -230,7 +230,7 @@ def calc_global_paths(fplist, csp):
dx = fp.x[i + 1] - fp.x[i]
dy = fp.y[i + 1] - fp.y[i]
fp.yaw.append(math.atan2(dy, dx))
fp.ds.append(math.sqrt(dx**2 + dy**2))
fp.ds.append(math.hypot(dx, dy))
fp.yaw.append(fp.yaw[-1])
fp.ds.append(fp.ds[-1])

2
PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py
View File

@@ -118,7 +118,7 @@ def find_sweep_direction_and_start_posi(ox, oy):
for i in range(len(ox) - 1):
dx = ox[i + 1] - ox[i]
dy = oy[i + 1] - oy[i]
d = np.sqrt(dx ** 2 + dy ** 2)
d = np.hypot(dx, dy)
if d > max_dist:
max_dist = d

6
PathPlanning/InformedRRTStar/informed_rrt_star.py
View File

@@ -106,7 +106,7 @@ class InformedRRTStar:
for i in nearInds:
dx = newNode.x - self.node_list[i].x
dy = newNode.y - self.node_list[i].y
d = math.sqrt(dx ** 2 + dy ** 2)
d = math.hypot(dx, dy)
theta = math.atan2(dy, dx)
if self.check_collision(self.node_list[i], theta, d):
dList.append(self.node_list[i].cost + d)
@@ -224,7 +224,7 @@ class InformedRRTStar:
if self.check_collision(nearNode, theta, d):
nearNode.parent = n_node - 1
nearNode.cost = scost
@staticmethod
def distance_squared_point_to_segment(v, w, p):
# Return minimum distance between line segment vw and point p
@@ -232,7 +232,7 @@ class InformedRRTStar:
return (p-v).dot(p-v) # v == w case
l2 = (w-v).dot(w-v) # i.e. |w-v|^2 - avoid a sqrt
# Consider the line extending the segment, parameterized as v + t (w - v).
# We find projection of point p onto the line.
# We find projection of point p onto the line.
# It falls where t = [(p-v) . (w-v)] / |w-v|^2
# We clamp t from [0,1] to handle points outside the segment vw.
t = max(0, min(1, (p - v).dot(w - v) / l2))

10
PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py
View File

@@ -97,7 +97,7 @@ def is_collision(sx, sy, gx, gy, rr, okdtree):
dx = gx - sx
dy = gy - sy
yaw = math.atan2(gy - sy, gx - sx)
d = math.sqrt(dx**2 + dy**2)
d = math.hypot(dx, dy)
if d >= MAX_EDGE_LEN:
return True
@@ -171,7 +171,7 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y):
road_map: ??? [m]
sample_x: ??? [m]
sample_y: ??? [m]
@return: Two lists of path coordinates ([x1, x2, ...], [y1, y2, ...]), empty list when no path was found
"""
@@ -182,7 +182,7 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y):
openset[len(road_map) - 2] = nstart
path_found = True
while True:
if not openset:
print("Cannot find path")
@@ -213,7 +213,7 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y):
n_id = road_map[c_id][i]
dx = sample_x[n_id] - current.x
dy = sample_y[n_id] - current.y
d = math.sqrt(dx**2 + dy**2)
d = math.hypot(dx, dy)
node = Node(sample_x[n_id], sample_y[n_id],
current.cost + d, c_id)
@@ -226,7 +226,7 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y):
openset[n_id].pind = c_id
else:
openset[n_id] = node
if path_found is False:
return [], []

4
PathPlanning/RRT/rrt.py
View File

@@ -126,7 +126,7 @@ class RRT:
def calc_dist_to_goal(self, x, y):
dx = x - self.end.x
dy = y - self.end.y
return math.sqrt(dx ** 2 + dy ** 2)
return math.hypot(dx, dy)
def get_random_node(self):
if random.randint(0, 100) > self.goal_sample_rate:
@@ -186,7 +186,7 @@ class RRT:
def calc_distance_and_angle(from_node, to_node):
dx = to_node.x - from_node.x
dy = to_node.y - from_node.y
d = math.sqrt(dx ** 2 + dy ** 2)
d = math.hypot(dx, dy)
theta = math.atan2(dy, dx)
return d, theta

4
PathPlanning/VoronoiRoadMap/voronoi_road_map.py
View File

@@ -95,7 +95,7 @@ def is_collision(sx, sy, gx, gy, rr, okdtree):
dx = gx - sx
dy = gy - sy
yaw = math.atan2(gy - sy, gx - sx)
d = math.sqrt(dx**2 + dy**2)
d = math.hypot(dx, dy)
if d >= MAX_EDGE_LEN:
return True
@@ -203,7 +203,7 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y):
n_id = road_map[c_id][i]
dx = sample_x[n_id] - current.x
dy = sample_y[n_id] - current.y
d = math.sqrt(dx**2 + dy**2)
d = math.hypot(dx, dy)
node = Node(sample_x[n_id], sample_y[n_id],
current.cost + d, c_id)

2
PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py
View File

@@ -212,7 +212,7 @@ def do_simulation(cx, cy, cyaw, ck, speed_profile, goal):
# check goal
dx = state.x - goal[0]
dy = state.y - goal[1]
if math.sqrt(dx ** 2 + dy ** 2) <= goal_dis:
if math.hypot(dx, dy) <= goal_dis:
print("Goal")
break

2
PathTracking/lqr_steer_control/lqr_steer_control.py
View File

@@ -191,7 +191,7 @@ def closed_loop_prediction(cx, cy, cyaw, ck, speed_profile, goal):
# check goal
dx = state.x - goal[0]
dy = state.y - goal[1]
if math.sqrt(dx ** 2 + dy ** 2) <= goal_dis:
if math.hypot(dx, dy) <= goal_dis:
print("Goal")
break

2
PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py
View File

@@ -348,7 +348,7 @@ def check_goal(state, goal, tind, nind):
# check goal
dx = state.x - goal[0]
dy = state.y - goal[1]
d = math.sqrt(dx ** 2 + dy ** 2)
d = math.hypot(dx, dy)
isgoal = (d <= GOAL_DIS)

4
PathTracking/move_to_pose/move_to_pose.py
View File

@@ -40,7 +40,7 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal):
x_traj, y_traj = [], []
rho = np.sqrt(x_diff**2 + y_diff**2)
rho = np.hypot(x_diff, y_diff)
while rho > 0.001:
x_traj.append(x)
y_traj.append(y)
@@ -52,7 +52,7 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal):
# [-pi, pi] to prevent unstable behavior e.g. difference going
# from 0 rad to 2*pi rad with slight turn
rho = np.sqrt(x_diff**2 + y_diff**2)
rho = np.hypot(x_diff, y_diff)
alpha = (np.arctan2(y_diff, x_diff)
- theta + np.pi) % (2 * np.pi) - np.pi
beta = (theta_goal - theta - alpha + np.pi) % (2 * np.pi) - np.pi

6
PathTracking/pure_pursuit/pure_pursuit.py
View File

@@ -77,7 +77,7 @@ def calc_distance(state, point_x, point_y):
dx = state.rear_x - point_x
dy = state.rear_y - point_y
return math.sqrt(dx ** 2 + dy ** 2)
return math.hypot(dx, dy)
def calc_target_index(state, cx, cy):
@@ -88,7 +88,7 @@ def calc_target_index(state, cx, cy):
# search nearest point index
dx = [state.rear_x - icx for icx in cx]
dy = [state.rear_y - icy for icy in cy]
d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)]
d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]
ind = d.index(min(d))
old_nearest_point_index = ind
else:
@@ -110,7 +110,7 @@ def calc_target_index(state, cx, cy):
while Lf > L and (ind + 1) < len(cx):
dx = cx[ind] - state.rear_x
dy = cy[ind] - state.rear_y
L = math.sqrt(dx ** 2 + dy ** 2)
L = math.hypot(dx, dy)
ind += 1
return ind

2
PathTracking/rear_wheel_feedback/rear_wheel_feedback.py
View File

@@ -136,7 +136,7 @@ def closed_loop_prediction(cx, cy, cyaw, ck, speed_profile, goal):
# check goal
dx = state.x - goal[0]
dy = state.y - goal[1]
if math.sqrt(dx ** 2 + dy ** 2) <= goal_dis:
if math.hypot(dx, dy) <= goal_dis:
print("Goal")
goal_flag = True
break

2
SLAM/EKFSLAM/ekf_slam.py
View File

@@ -76,7 +76,7 @@ def observation(xTrue, xd, u, RFID):
dx = RFID[i, 0] - xTrue[0, 0]
dy = RFID[i, 1] - xTrue[1, 0]
d = math.sqrt(dx ** 2 + dy ** 2)
d = math.hypot(dx, dy)
angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0])
if d <= MAX_RANGE:
dn = d + np.random.randn() * Q_sim[0, 0] ** 0.5 # add noise

2
SLAM/FastSLAM1/fast_slam1.py
View File

@@ -279,7 +279,7 @@ def observation(xTrue, xd, u, RFID):
dx = RFID[i, 0] - xTrue[0, 0]
dy = RFID[i, 1] - xTrue[1, 0]
d = math.sqrt(dx ** 2 + dy ** 2)
d = math.hypot(dx, dy)
angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0])
if d <= MAX_RANGE:
dn = d + np.random.randn() * Qsim[0, 0] ** 0.5 # add noise

2
SLAM/FastSLAM2/fast_slam2.py
View File

@@ -304,7 +304,7 @@ def observation(xTrue, xd, u, RFID):
dx = RFID[i, 0] - xTrue[0, 0]
dy = RFID[i, 1] - xTrue[1, 0]
d = math.sqrt(dx ** 2 + dy ** 2)
d = math.hypot(dx, dy)
angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0])
if d <= MAX_RANGE:
dn = d + np.random.randn() * Q_sim[0, 0] ** 0.5 # add noise

2
SLAM/GraphBasedSLAM/graph_based_slam.py
View File

@@ -216,7 +216,7 @@ def observation(xTrue, xd, u, RFID):
dx = RFID[i, 0] - xTrue[0, 0]
dy = RFID[i, 1] - xTrue[1, 0]
d = math.sqrt(dx**2 + dy**2)
d = math.hypot(dx, dy)
angle = pi_2_pi(math.atan2(dy, dx)) - xTrue[2, 0]
phi = pi_2_pi(math.atan2(dy, dx))
if d <= MAX_RANGE: