Particle Filter - Kidnapped vehicle project

1. Definition of Particle Filter

粒子濾波器是貝葉斯濾波器或馬爾可夫定位濾波器的實(shí)現(xiàn)。粒子過(guò)濾器基于“適者生存的原理”主要用于解決定位問(wèn)題。粒子濾波的優(yōu)勢(shì)在于易于編程并且靈活。 三種濾波器的性能對(duì)比:

?

? 正如你在上面的圖片中看到的，紅點(diǎn)是對(duì)機(jī)器人可能位置的離散猜測(cè)。每個(gè)紅點(diǎn)都有 x 坐標(biāo)、 y 坐標(biāo)和方向。粒子濾波器是由幾千個(gè)這樣的猜測(cè)組成的機(jī)器人后驗(yàn)信度表示。一開(kāi)始，粒子是均勻分布的，但過(guò)濾器使他們生存的比例正比于粒子與傳感器測(cè)量的一致性。

權(quán)重(Weights)：

粒子濾波器通常攜帶離散數(shù)量的粒子。每個(gè)粒子都是一個(gè)包含 x 坐標(biāo)、 y 坐標(biāo)和方向的矢量。顆粒的存活取決于它們與傳感器測(cè)量結(jié)果的一致性。一致性是基于實(shí)際測(cè)量和預(yù)測(cè)測(cè)量之間的匹配度來(lái)衡量的，這種匹配度稱為權(quán)重。 ?

? 權(quán)重意味著粒子的實(shí)際測(cè)量與預(yù)測(cè)測(cè)量的接近程度。在粒子濾波器中，粒子權(quán)重越大，生存概率越高。換句話說(shuō)，每個(gè)粒子的生存概率與權(quán)重成正比。 ?

重采樣(Resampling)

重采樣技術(shù)是用于從舊粒子中隨機(jī)抽取N個(gè)新粒子，并根據(jù)重要權(quán)重按比例進(jìn)行置換。重采樣后，權(quán)重較大的粒子可能會(huì)停留下來(lái)，其他粒子可能會(huì)消失。粒子聚集在后驗(yàn)概率比較高的區(qū)域。 為了進(jìn)行重采樣，采用了重采樣輪技術(shù). ?

?

? 原理：每個(gè)粒子被選中的概率都和這個(gè)粒子輪所占的周長(zhǎng)成正比，權(quán)重大的粒子有更多的機(jī)會(huì)被選中。 初始index為6，假設(shè)隨機(jī)的beta= 0 + 隨機(jī)權(quán)重> w6, 則index +1，beta=beta-w6. 此時(shí)beta < w7, 7號(hào)粒子被選中添加到倉(cāng)庫(kù)中。之后進(jìn)行下一輪循環(huán)，此時(shí)beta 和 index 仍然保留前一輪循環(huán)的值, beta= beta + 隨機(jī)權(quán)重 > w7 + w8, 因此index遞增兩次，到達(dá)index=1，此時(shí)w1 > beta, w1被選中放入倉(cāng)庫(kù)中, 隨后進(jìn)行下一輪循環(huán)。 重采樣的代碼：

p3 = []
index= int(random.random()*N)
beta=0.0
mw=max(w)
for i in range(N):
    beta +=random.random()*2.0*mw
    while beta>w[index]:
        beta-=w[index]
        index=(index+1)%N
    p3.append(p[index])
p=p3

?2. Particle Filters implementation

? 粒子過(guò)濾器有四個(gè)主要步驟:

初始化步驟: 我們從 GPS 輸入估計(jì)我們的位置。在這個(gè)過(guò)程中的后續(xù)步驟將完善這個(gè)估計(jì)，以定位我們的車輛

預(yù)測(cè)步驟: 在預(yù)測(cè)步驟中，我們添加了所有粒子的控制輸入(偏航速度和速度)

粒子權(quán)重更新步驟: 在更新步驟中，我們使用地圖地標(biāo)位置和特征的測(cè)量更新粒子權(quán)重

重采樣步驟: 在重采樣期間，我們將重采樣 m 次(m 是0到 length_of_particleArray的范圍)繪制粒子 i (i 是粒子index)與其權(quán)重成正比。這一步使用了重采樣輪技術(shù)。

新的粒子代表了貝葉斯濾波后驗(yàn)概率。我們現(xiàn)在有一個(gè)基于輸入證明的車輛位置的精確估計(jì)。

偽代碼：

1. 初始化步驟:

粒子過(guò)濾器的第一件事就是初始化所有的粒子。在這一步，我們必須決定要使用多少粒子。一般來(lái)說(shuō)，我們必須拿出一個(gè)好的數(shù)字，因?yàn)槿绻粫?huì)太小，將容易出錯(cuò)，如果太多會(huì)拖慢貝葉斯濾波器的速度。傳統(tǒng)的粒子初始化方式是把狀態(tài)空間劃分成一個(gè)網(wǎng)格，并在每個(gè)單元格中放置一個(gè)粒子，但這種方式只能適合小狀態(tài)空間，如果狀態(tài)空間是地球，這是不合適的。因此用 GPS位置輸入來(lái)初始估計(jì)我們的粒子分布是最實(shí)用的。值得注意的是，所有傳感器的測(cè)量結(jié)果必然伴隨著噪聲，為了模擬真實(shí)的噪聲不可控情況，應(yīng)考慮給本項(xiàng)目的初始 GPS 位置和航向添加高斯噪聲。 項(xiàng)目的最終初始化步驟代碼：
void ParticleFilter::init(double x, double y, double theta, double std[]) {

    /**
     * TODO: Set the number of particles. Initialize all particles to
     *   first position (based on estimates of x, y, theta and their uncertainties
     *   from GPS) and all weights to 1.
     * TODO: Add random Gaussian noise to each particle.
     * NOTE: Consult particle_filter.h for more information about this method
     *   (and others in this file).
     */

    if (is_initialized) {
        return;
    }
    num_particles = 100;  // TODO: Set the number of particle

    double std_x = std[0];
    double std_y = std[1];
    double std_theta = std[2];

    // Normal distributions
    normal_distribution dist_x(x, std_x);
    normal_distribution dist_y(y, std_y);
    normal_distribution dist_theta(theta, std_theta);

    // Generate particles with normal distribution with mean on GPS values.
    for (int i = 0; i < num_particles; ++i) {
        Particle pe;
        pe.id = i;
        pe.x = dist_x(gen);
        pe.y = dist_y(gen);
        pe.theta = dist_theta(gen);
        pe.weight = 1.0;
        particles.push_back(pe);
        
    }
    is_initialized = true;
}

? 2. 預(yù)測(cè)步驟： 現(xiàn)在我們已經(jīng)初始化了粒子，是時(shí)候預(yù)測(cè)車輛的位置了。在這里，我們將使用下面的公式來(lái)預(yù)測(cè)車輛將在下一個(gè)時(shí)間步驟，通過(guò)基于偏航速度和速度的更新，同時(shí)考慮高斯傳感器噪聲。 ?

? 項(xiàng)目的最終預(yù)測(cè)步驟代碼:

for (int i = 0; i < num_particles; i++) { if (fabs(yaw_rate) >= 0.00001) { particles[i].x += (velocity / yaw_rate) * (sin(particles[i].theta + yaw_rate * delta_t) - sin(particles[i].theta)); particles[i].y += (velocity / yaw_rate) * (cos(particles[i].theta) - cos(particles[i].theta + yaw_rate * delta_t)); particles[i].theta += yaw_rate * delta_t; } else { particles[i].x += velocity * delta_t * cos(particles[i].theta); particles[i].y += velocity * delta_t * sin(particles[i].theta); } // Add noise particles[i].x += disX(gen); particles[i].y += disY(gen); particles[i].theta += angle_theta(gen); }

3. 更新步驟:

現(xiàn)在，我們已經(jīng)將速度和偏航率測(cè)量輸入納入到我們的過(guò)濾器中，我們必須更新基于激光雷達(dá)和雷達(dá)地標(biāo)讀數(shù)的粒子權(quán)重。 更新步驟有三個(gè)主要步驟:

Transformation

Association

Update Weights

轉(zhuǎn)換 (Transformation) 我們首先需要將汽車的測(cè)量數(shù)據(jù)從當(dāng)?shù)氐钠囎鴺?biāo)系轉(zhuǎn)換為地圖上的坐標(biāo)系。

通過(guò)傳遞車輛觀測(cè)坐標(biāo)(xc 和 yc)、地圖粒子坐標(biāo)(xp 和 yp)和我們的旋轉(zhuǎn)角度(- 90度) ，通過(guò)齊次變換矩陣，車輛坐標(biāo)系中的觀測(cè)值可以轉(zhuǎn)換為地圖坐標(biāo)(xm 和 ym) 。這個(gè)齊次的變換矩陣，如下所示，執(zhí)行旋轉(zhuǎn)和平移。

矩陣乘法的結(jié)果是:

代碼

 double x_part, y_part, x_obs, y_obs, theta;
  double x_map;
 x_map = x_part + (cos(theta) * x_obs) - (sin(theta) * y_obs);
  double y_map;
 y_map = y_part + (sin(theta) * x_obs) + (cos(theta) * y_obs);

備注: 黑色方框是一個(gè)粒子，我們需要更新他的權(quán)重，(4,5) 是它在地圖坐標(biāo)中的位置，它的航向是(-90度)，由于傳感器對(duì)路標(biāo)的測(cè)量結(jié)果是基于車輛本身坐標(biāo)，因此我們要把車輛的觀察數(shù)據(jù)轉(zhuǎn)換為地圖坐標(biāo)。如L1路標(biāo)的真實(shí)地圖坐標(biāo)是(5,3)，車輛傳感器測(cè)得的OBS2的車輛坐標(biāo)為(2,2), 經(jīng)過(guò)齊次矩陣轉(zhuǎn)換后的地圖坐標(biāo)是(6,3), 現(xiàn)在我們就可以將測(cè)量結(jié)果與真實(shí)結(jié)果聯(lián)系起來(lái)，匹配現(xiàn)實(shí)世界中的地標(biāo), 從而更新黑色方框粒子的權(quán)重。
?聯(lián)系 (Association )

聯(lián)系問(wèn)題是在現(xiàn)實(shí)世界中地標(biāo)測(cè)量與物體匹配的問(wèn)題，如地圖地標(biāo). 我們的最終目標(biāo)是為每個(gè)粒子找到一個(gè)權(quán)重參數(shù)，這個(gè)權(quán)重參數(shù)代表這個(gè)粒子與實(shí)際汽車在同一位置的匹配程度。

現(xiàn)在已經(jīng)將觀測(cè)值轉(zhuǎn)換為地圖的坐標(biāo)空間，下一步是將每個(gè)轉(zhuǎn)換后的觀測(cè)值與一個(gè)地標(biāo)識(shí)符關(guān)聯(lián)起來(lái)。在上面的地圖練習(xí)中，我們總共有5個(gè)地標(biāo)，每個(gè)都被確定為 L1，L2，L3，L4，L5，每個(gè)都有一個(gè)已知的地圖位置。我們需要將每個(gè)轉(zhuǎn)換觀察 TOBS1，TOBS2，TOBS3與這5個(gè)識(shí)別符之一聯(lián)系起來(lái)。為了做到這一點(diǎn)，我們必須將最接近的地標(biāo)與每一個(gè)轉(zhuǎn)化的觀察聯(lián)系起來(lái)。
TOBS1?= (6,3),?TOBS2?= (2,2) and?TOBS3?= (0,5). OBS1匹配L1，OBS2匹配L2，OBS3匹配L2或者L5(距離相同)。
下面的例子來(lái)解釋關(guān)于數(shù)據(jù)關(guān)聯(lián)的問(wèn)題。

在這種情況下，我們有兩個(gè)激光雷達(dá)測(cè)量巖石。我們需要找出這兩個(gè)測(cè)量值中，哪一個(gè)與巖石相對(duì)應(yīng)。如果我們估計(jì)，任何測(cè)量是真實(shí)的，汽車的位置將根據(jù)我們選擇的測(cè)量是不同的。也就是說(shuō)，根據(jù)路標(biāo)選擇的不同, 最終確定的車輛位置也會(huì)不同。
由于我們有多個(gè)測(cè)量的地標(biāo)，我們可以使用最近鄰技術(shù)找到正確的一個(gè)。

在這種方法中，我們將最接近的測(cè)量作為正確的測(cè)量。

最緊鄰法的優(yōu)缺點(diǎn):

更新權(quán)重 (update weights)
現(xiàn)在我們已經(jīng)完成了測(cè)量轉(zhuǎn)換和關(guān)聯(lián)，我們有了計(jì)算粒子最終權(quán)重所需的所有部分。粒子的最終權(quán)重將計(jì)算為每個(gè)測(cè)量的多元-高斯概率密度的乘積。
多元-高斯概率密度有兩個(gè)維度，x 和 y。多元高斯分布的均值是測(cè)量的相關(guān)地標(biāo)位置，多元高斯分布的標(biāo)準(zhǔn)差是由我們?cè)?x 和 y 范圍內(nèi)的初始不確定度來(lái)描述的。多元-高斯的評(píng)估基于轉(zhuǎn)換后的測(cè)量位置。多元高斯分布的公式如下。

備注：x, y 是地圖坐標(biāo)系的觀測(cè)值，μx,?μy是最近的路標(biāo)的地圖坐標(biāo)。如對(duì)于OBS2 (x,y) =(2,2), (μx,?μy)= (2,1)
誤差計(jì)算公式：

?

其中x, y, theta代表最好的粒子位置，xmeas, ymeas, thetameas代表真實(shí)值

?

?

?

3. Project Demo

編輯：黃飛

?