github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-15 07:38:03 -05:00
Code Issues Packages Projects Releases Wiki Activity

remove np.matrix from histogram_filter.py

Browse Source
This commit is contained in:
Atsushi Sakai
2018-12-05 20:59:46 +09:00
parent c9a9e48f72
commit d7fe083b1c
1 changed files with 13 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
Localization/histogram_filter/histogram_filter.py
View File

@@ -94,23 +94,23 @@ def observation_update(gmap, z, std):
def calc_input():
v = 1.0 # [m/s]
yawrate = 0.1 # [rad/s]
u = np.matrix([v, yawrate]).T
u = np.array([v, yawrate]).reshape(2, 1)
return u
def motion_model(x, u):
F = np.matrix([[1.0, 0, 0, 0],
[0, 1.0, 0, 0],
[0, 0, 1.0, 0],
[0, 0, 0, 0]])
F = np.array([[1.0, 0, 0, 0],
[0, 1.0, 0, 0],
[0, 0, 1.0, 0],
[0, 0, 0, 0]])
B = np.matrix([[DT * math.cos(x[2, 0]), 0],
[DT * math.sin(x[2, 0]), 0],
[0.0, DT],
[1.0, 0.0]])
B = np.array([[DT * math.cos(x[2, 0]), 0],
[DT * math.sin(x[2, 0]), 0],
[0.0, DT],
[1.0, 0.0]])
x = F * x + B * u
x = F @ x + B @ u
return x
@@ -125,7 +125,7 @@ def observation(xTrue, u, RFID):
xTrue = motion_model(xTrue, u)
z = np.matrix(np.zeros((0, 3)))
z = np.zeros((0, 3))
for i in range(len(RFID[:, 0])):
@@ -135,7 +135,7 @@ def observation(xTrue, u, RFID):
if d <= MAX_RANGE:
# add noise to range observation
dn = d + np.random.randn() * NOISE_RANGE
zi = np.matrix([dn, RFID[i, 0], RFID[i, 1]])
zi = np.array([dn, RFID[i, 0], RFID[i, 1]])
z = np.vstack((z, zi))
# add noise to speed
@@ -225,7 +225,7 @@ def main():
time = 0.0
xTrue = np.matrix(np.zeros((4, 1)))
xTrue = np.zeros((4, 1))
gmap = init_gmap(XY_RESO, MINX, MINY, MAXX, MAXY)
mx, my = calc_grid_index(gmap) # for grid map visualization