在前面，我们介绍了基于一维网格地图的直方图滤波算法，今天我们将它扩展到二维网格地图，以进一步理解算法。正如前面所提到的那样，整个滤波的过程包含两个部分， 测量更新和 运动更新最后得到网格地图的概率分布，以表示机器人目前处于的位置。
下面让我们讲解基于2维地图的直方图滤波算法的实现。
首先，我们需要初始化各个网格的初始概率：
目录
初始化
# initializes p to a uniform distribution over a grid of the same dimensions as colors pinit = 1.0 / float(len(colors)) / float(len(colors[0])) p = [[pinit for row in range(len(colors[0]))] for col in range(len(colors))]其中 colors应该定义为下面的形式以表示每一网格的颜色信息
colors = [[R, G], [R, R], [G, R], [R, G], [G, G]]sense function
然后是我们的 sense 函数：
Nr=len(colors) Nc=len(colors[0]) def sense(p, Z): q=[[p[i][j]*sensor_right if colors[i][j]==Z else p[i][j]*(1-sensor_right) for j in range(Nc) ] for i in range(Nr)] sumq=sum([sum(e) for e in q]) q=[[x/sumq for x in e] for e in q] return q与一维的网格地图一样，分别遍历每一个网格，并且根据测量值（Z）是否与地图值（colors[i][j]）相同以分别乘以相应的概率，然后进行求和，整个过程为一个卷积操作，然后进行归一化，得到由测量引起的概率分布。
move function
motions = [[0, 0], [-1, 0], [0, 1], [0, -1], [0, 1], [1, 0]] def move(p, U): q=[[ p_move*p[(i-U[0])%Nr][(j-U[1])%Nc]+(1-p_move)*p[i][j] for j in range(Nc) ] for i in range(Nr)] return q这里为了简化情况，只考虑了一个 p_move 即机器人是否移动的概率，然后与sense 函数一样，对所有网格进行遍历，根据定义的运动模型，分别乘上相应的概率，最后得到运动更新之后的概率分布。
输出
我们根据定义的观测值，计算最后的概率分布
colors = [[R, G], [R, R], [G, R], [R, G], [G, G]] measurements = [R, R, G, G, G, R] motions = [[0, 0], [-1, 0], [0, 1], [0, -1], [0, 1], [1, 0]] p = localize(colors,measurements,motions,sensor_right=0.99, p_move=0.97) print(Estimated probability at each location:) show(p)最终得到的输出如下：
Estimated probability at each location:[[0.07876,0.00793],[0.02465,0.85350], [0.00001,0.00004],[0.03447,0.00002], [0.00003,0.00058]]