From 7fbfe5b41a2a4c14c455287e5bcfbc200a6f2aa2 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sat, 17 Mar 2018 11:05:03 -0700 Subject: [PATCH] keep coding --- .../histogram_filter/histogram_filter.py | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/Localization/histogram_filter/histogram_filter.py b/Localization/histogram_filter/histogram_filter.py index 4de8df8a..f3ef5853 100644 --- a/Localization/histogram_filter/histogram_filter.py +++ b/Localization/histogram_filter/histogram_filter.py @@ -66,7 +66,9 @@ def motion_model(x, u): def draw_heatmap(data, minx, maxx, miny, maxy, xyreso): x, y = np.mgrid[slice(minx - xyreso / 2.0, maxx + xyreso / 2.0, xyreso), slice(miny - xyreso / 2.0, maxy + xyreso / 2.0, xyreso)] - plt.pcolor(x, y, data, vmax=1.0, cmap=plt.cm.Blues) + + maxp = max([max(igmap) for igmap in data]) + plt.pcolor(x, y, data, vmax=maxp, cmap=plt.cm.Blues) plt.axis("equal") @@ -93,7 +95,7 @@ def observation(xTrue, u, RFID): def normalize_probability(gmap): sump = sum([sum(igmap) for igmap in gmap]) - # print(sump) + print(sump) for i in range(len(gmap)): for ii in range(len(gmap[i])): @@ -117,6 +119,15 @@ def init_gmap(xyreso): return gmap, minx, maxx, miny, maxy, +def motion_update(gmap, u, yaw, xyreso, minx, miny): + + dx = DT * math.cos(yaw) + dy = DT * math.sin(yaw) + print(dx, dy) + + return gmap + + def main(): print(__file__ + " start!!") @@ -135,12 +146,16 @@ def main(): gmap, minx, maxx, miny, maxy = init_gmap(xyreso) + dx, dy = 0.0, 0.0 + while SIM_TIME >= time: time += DT u = calc_input() xTrue, z = observation(xTrue, u, RFID) + gmap = motion_update(gmap, u, xTrue[2, 0], xyreso, minx, miny) + gmap = observation_update(gmap, z, STD, xyreso, minx, miny) if show_animation: