Question

我正在尝试实施一种用于机器人定位的离散贝叶斯滤波器（即直方图滤波器），如Thrun，Burgard和Fox的'Probabilistic Robotics'中所述。该模型是一个在一维中移动的机器人，因此状态向量就是位置和速度。在每个时间步都有一个随机加速度，均值为零，方差为1。

我认为我的实施是准确的，但运行缓慢。我正在循环我的概率密度图的索引，这似乎非常低效，但我没有看到如何正确地矢量化这个算法。有什么建议吗？

我的代码：

import matplotlib.pyplot as plt
import numpy as np
from math import pi, exp, sqrt
import matplotlib.cm as cm

N = 31
numsteps = 5

# set up grid
x = np.linspace(-10, 10, N)
v = np.linspace(-10, 10, N)
sp = (x[1] - x[0])/2
X, V = np.meshgrid(x, v)

# initial probability distribution is a delta function at origin
p0 = np.zeros(X.shape)
p0[N/2, N/2] = 1
plt.ion()
plt.imshow(p0, cmap = cm.Greys_r, interpolation='none')
plt.draw()

acc_var = 1 # Variance of random acceleration

for ii in range(numsteps):
    print 'Step %d'%ii
    p1 = np.zeros(X.shape)
    for (i, j), p in np.ndenumerate(p0): #outer loop over configuration space
        for (k,l), x in np.ndenumerate(X):
            # new position is old position plus velocity
            if X[i,j] > X[k,l] + V[k,l] - sp and X[i,j] <= X[k,l] + V[k,l] + sp: 
                # Bayesian update with random acceleration
                p1[i,j] = p1[i,j] + exp(-0.5 * (V[i,j] - V[k,l])**2 / acc_var) / sqrt(2*pi*acc_var) * p0[k,l] 
    p0 = p1
    plt.imshow(p1, cmap = cm.Greys_r, interpolation='none')
    plt.draw()

高效的离散贝叶斯滤波器用于定位

0 个答案: