粒子濾波定位

理論知識

除了線性狀態估計的KF和非線性狀態估計的EKF，還有一種可以解決非線性、非高斯問題的粒子濾波算法，粒子濾波主要基於蒙特卡洛方法，使用粒子集來表示概率。
粒子濾波主要分爲四部分：初始化、預測、粒子權重更新、重採樣，之後在重複的預測、更新、重採樣，使得粒子逐漸向真實位置聚集。

1、初始化

粒子濾波初始化需要初始的位置（x,y），航向(yaw)，以及高斯噪聲。

單純用GPS的結果是有很大誤差的，所以這邊在初始化時，創造出N個粒子，並且所有粒子都是在GPS提供的初值附近的高斯分佈。這是黑色圓圈出現的原因。每個粒子權重都相等，都是100%。
利用GPS的值初始化一定量粒子，粒子的**位置（x, y）, 航向角（yaw）**滿足正態分佈。GPS測量值爲均值，上面黃框中的傳感器噪音，即爲分佈方差。

2、預測

汽車的定位模塊能夠受到自車的車速v和航向角變化率yaw_rate，進而可以推測出運動△t時間後，自車所在的位置。由於每個粒子都可能是無人車的真實位置，當我們對所有粒子進行里程計預測（考慮運動噪音，也是滿足正太分佈），每個粒子都會有自己的位置和航向角。

接下來由於涉及了航向角，我們先介紹汽車的運動模型CTRV（恆定轉向速率yaw_rate和速度v）。

狀態向量爲[px, py, v, yaw, yaw_rate]，位置(px,py)，速度(v)方向爲運動軌跡切線，yaw爲航向角，yaw_rate爲航向角變化率。

由於行駛速度v和航向角的變化率φ’爲常量，v的導數和φ’'均爲0

根據航向角的變化率φ’是否爲0，分爲兩種：
1、當φ’爲0，即速度、航向角變化率、航向角均不發生變化，汽車此時沿直線行駛，只有位置發生變化。

2、當φ’不爲0，是個常量，k和k+1時間段內汽車做圓周運動，狀態方程爲：

3、更新粒子權重

這些粒子哪些才能更好的代表汽車的位置了，所以要給它們相應的權重，權重越大，它被重採樣的機會也就越大，這樣隨着不但迭代，最終會找到最能代表汽車位置的粒子了。

1、座標系轉換（從車系Oc到世界系Om）
汽車傳感器會獲得周圍路標的位置(比如a,b,c三個路標)，將這些汽車座標下的位置(比如a,b,c)轉化爲全局地圖上的位置（a’, b’ ,c’）


2、數據關聯、“臨近法” 通過id綁定觀測到的路標與地圖中實際存在的路標

**3、將某粒子下觀測到的所有路標的可信度累乘，就可以得到這個粒子的權重了 w=P(xa, ya)P(xb, yb)P(xc, yc)
利用聯合高斯分佈，求出每個路標的可信度P(x, y)。其中x, y爲測量值，μx, μy 爲對應路標地圖上的位置。

4、重採樣

重採樣的目的是，保持粒子總數不變的情況下，刪除掉那些權重較低的粒子，同時在權重較高的粒子附近拋灑新的粒子。對這些不同權重的粒子進行重新選取，權重大的粒子，代表的位置準確度更高，所以應該更多的重採樣這些權重大的粒子。

隨着重採樣次數的增加，粒子的集中度會越來越高，留下來的都是極其接近真值的粒子。在這些粒子中篩選出權重最高的粒子，即爲無人車的位置。

代碼講解

1、初始化init()

粒子濾波初始化需要一個初始的位置、航向、測量誤差。
利用GPS的值作爲均值進行初始化。粒子的x,y,yaw都滿足高斯分佈，將均值和噪聲作爲已知量初始化N個粒子。
函數init()的聲明以及各參數含義如下：

• @param x 初始的x（GPS估計值）
• @param y 初始的y
• @param theta 初始航向角[rad]
• @param std[] 上述3個的標準差

void init(double x, double y, double theta, double std[]);

/**
   * TODO: 設置粒子數目，初始化所有粒子在第一個位置的高斯分佈，且所有權重都是1 
   * TODO: 對每個粒子在初始位置和初始航向的基礎上加入一個隨機的高斯分佈測量誤差
   */
void ParticleFilter::init(double x, double y, double theta, double std[]) {
    // 粒子數目N
    num_particles = 100;
    particles.resize(num_particles);

    // 根據均值和測量噪聲建立呈現高斯分佈的N個粒子
    std::default_random_engine gen;
    std::normal_distribution<double> dist_x(x, std[0]);// 均值和方差
    std::normal_distribution<double> dist_y(y, std[1]);
    std::normal_distribution<double> dist_theta(theta, std[2]);
    // 遍歷所有粒子
    for (size_t i = 0; i < num_particles; ++i) {
        particles.at(i).x = dist_x(gen);
        particles.at(i).y = dist_y(gen);
        particles.at(i).theta = dist_theta(gen);
        particles.at(i).weight = 1.0;// 權重都是1
    }
    // 完成了初始化
    is_initialized = true;
}

2、預測prediction()

CRTV模型(圓周運動)：恆定轉向速率yaw_rate和速度v模型。狀態方程爲[px, py, v, yaw, yaw_rate]，其中px,py表示無人車的位置，v表示速度，方向爲運動軌跡的切線方向。yaw表示航向角、yaw_rate表示航向角的變化率。

直接亮結論，根據航向角變化率是否爲0分爲兩種：
1、當φ’爲0，即速度、航向角變化率、航向角均不發生變化，汽車此時沿直線行駛，只有位置發生變化。

2、當φ’不爲0，是個常量，k和k+1時間段內汽車做圓周運動，狀態方程爲：

函數prediction()的聲明以及各參數含義爲：

/** 預測每個粒子下一時刻狀態

• @param delta_t 兩幀時間差
• @param std_pos[]預測時所引入的預測噪聲，3維（x，y，yaw）
• @param velocity 速度[m/s]
• @param yaw_rate 航向角變化率 [rad/s] */

void prediction(double delta_t, double std_pos[], double velocity, double yaw_rate);

// 預測每個粒子下一時刻狀態，最後要加上高斯噪聲
void ParticleFilter::prediction(double delta_t, double std_pos[], double velocity, double yaw_rate) {

    // 生成均值爲0的純高斯噪聲
    std::default_random_engine gen;
    std::normal_distribution<double> noisy_x(0, std_pos[0]);
    std::normal_distribution<double> noisy_y(0, std_pos[1]);
    std::normal_distribution<double> noisy_theta(0, std_pos[2]);

    // 遍歷所有粒子
    for (size_t i = 0; i < num_particles; ++i) {
        // 1. k時刻
        double x0 = particles.at(i).x;
        double y0 = particles.at(i).y;
        double theta0 = particles.at(i).theta;

        double x_pre = 0.0;
        double y_pre = 0.0;
        double theta_pre = 0.0;
        // 2. k+1時刻
        // 如果航向角變化率爲0，直線行駛
        if (fabs(yaw_rate) < 0.00001) {
            x_pre = x0 + velocity * delta_t * cos(theta0);
            y_pre = y0 + velocity * delta_t * sin(theta0);
            theta_pre = theta0;
        } 
        // 航向角變換率不是0，做圓周運動
        else {
            x_pre = x0 + velocity / yaw_rate * (sin(theta0 + yaw_rate * delta_t) - sin(theta0));
            y_pre = y0 + velocity / yaw_rate * (cos(theta0) - cos(theta0 + yaw_rate * delta_t));
            theta_pre = theta0 + yaw_rate * delta_t;
        }
        // 判斷
        while (theta_pre > 2 * M_PI) theta_pre -= 2. * M_PI;
        while (theta_pre < 0.0) theta_pre += 2. * M_PI;
        // 3. 加噪聲
        particles.at(i).x = x_pre + noisy_x(gen);
        particles.at(i).y = y_pre + noisy_y(gen);
        particles.at(i).theta = theta_pre + noisy_theta(gen);
    }
}

3、更新粒子權重updateWeights()

從代碼中看出更新粒子權重主要分爲三部分，一、將車體座標系下觀測到的路標轉化到平面座標系下；二、數據關聯：將平面座標系下觀測到的路標和真實路標匹配；三、根據匹配的結果重新計算粒子權重。
函數updateWeights()的聲明和各參數含義爲：

/** * 根據觀測到的測量值的可能性更新每個粒子的權重

• @param sensor_range 傳感器觀測範圍 [m]
• @param std_landmark[] 觀測時引入的觀測噪聲，2維（x，y）
• @param observations 路標觀測值
• @param map Map地圖中真實的路標值 */

void updateWeights(double sensor_range, double std_landmark[], const std::vector<LandmarkObs> &observations, const Map &map_landmarks);

1、座標系轉換（從車系Oc到世界系Om）：
傳感器得到的數據是車體座標系Oc下觀測到的路標位置(xc,yc)，我們要將其轉化爲平面座標系(xm,ym),公式如下：


這一步目的是爲dataAssociation()函數準備兩個參數（map_in_range，obs_in_map）
首先獲取車體在平面座標系下座標（xp,yp,theta），然後根據車體位置找到離車體最近的幾個路標點map_in_range，之後把車體座標系觀測到的路標轉化到平面座標系下obs_in_map。

// 1.獲取視覺範圍內地圖上的路標集map_in_range和經過座標轉換後的觀測路標obs_in_map                             
        // 1.1獲取車體在平面座標系下座標（xp,yp,theta）
        for (size_t i = 0; i < num_particles; ++i) {
        double xp = particles.at(i).x;
        double yp = particles.at(i).y;
        double theta = particles.at(i).theta;

        // 1.2粗略的傳感器觀測範圍內的路標map_in_range， with land mark id
        // 根據距離，找到離當前點最近的點作爲關聯點。
        vector<LandmarkObs> map_in_range;
        double distance_threshold = sensor_range * sensor_range;
        for (auto land_mark : map_landmarks.landmark_list) {
            double dis2 = (xp - land_mark.x_f) * (xp - land_mark.x_f) + (yp - land_mark.y_f) * (yp - land_mark.y_f);
            if (dis2 <= distance_threshold) {
                LandmarkObs map_obs{land_mark.id_i, land_mark.x_f, land_mark.y_f};
                map_in_range.push_back(map_obs);
            }
        }
        // 1.3將車體座標系觀測到的粒子轉化到平面座標系下，沒有ID 0
        vector<LandmarkObs> obs_in_map(observations.size());
        for (size_t j = 0; j < observations.size(); ++j) {
            double xc = observations[j].x;
            double yc = observations[j].y;
            obs_in_map[j].x = xp + (cos(theta) * xc) - (sin(theta) * yc);
            obs_in_map[j].y = yp + (sin(theta) * xc) + (cos(theta) * yc);
            obs_in_map[j].id = observations[j].id;
        }

2、數據關聯、通過id綁定觀測到的路標與地圖中實際存在的路標
函數dataAssociation()聲明和參數定義爲：

/**    *   經過數據關聯後，每個觀測到的路標會與實際的路標通過id進行綁定。    
* @param predicted 視覺範圍內地圖上的真實路標集    
* @param observations 經過座標轉換後的觀測路標

void dataAssociation(std::vector predicted, std::vector& observations);

這步是爲了找到觀測到的路標和地圖中實際的路標真值對應關係，“最臨近法”就是根據距離找關聯點。對於每個觀測值，遍歷所有的地圖真值路標，找到距離最小的路標，並把該地圖真值路標的ID賦值給觀測的路標，從而建立關聯關係。
下圖中，紅圈標註的是和Map 路標最接近的雷達測量值。即可把紅圈內的測量值路標和真實路標ID綁定上。

/**
   * TODO: 每個車體都有觀測的粒子結果，爲每個粒子匹配最相似的路標。
   * 找到最接近每個觀測到的測量值的預測測量結果，並將觀察到的測量值分配給該特定路標
    */
// 經過數據關聯後，每個觀測到的路標會與實際的路標通過id進行綁定。
void ParticleFilter::dataAssociation(vector<LandmarkObs> predicted, 
                                     vector<LandmarkObs>& observations) {
    // 遍歷觀測到的路標
    for (LandmarkObs& obs : observations) {
        double distance_min = std::numeric_limits<double>::max();
        // 遍歷所有地圖上真值路標
        for (int i = 0; i < predicted.size(); ++i) {
            LandmarkObs pre = predicted[i];
            double distance2 = (pre.x - obs.x) * (pre.x - obs.x) + (pre.y - obs.y) * (pre.y - obs.y);
            if (distance2 < distance_min) {
                distance_min = distance2;
                obs.id = pre.id;// 綁定ID
            }
        }
    }
}

3、計算粒子權重
完成數據關聯後，我們會發現每一個觀測到的路標，都與真實世界中的某個路標有對應關係。至此，我們就可以根據他們之間的距離關係計算出一個權值，這個權值能夠反映當前位置觀測到的路標與真實世界中的路標匹配地是否足夠好。

P(x,y)表示匹配概率，x與ux、y與uy越接近，P(x,y)越大。
首先根據數據關聯的ID找到地圖中真實的路標，計算二者之間概率P(x,y)，最後有多少觀測路標就連續乘多少次權值，最終得到每個粒子對應的權重。

        // 3.更新粒子權重

        // 3.1 2維觀測噪聲和粒子權重
        double std_x = std_landmark[0];
        double std_y = std_landmark[1];
        particles.at(i).weight = 1.0;

        // 3.2計算粒子權重
        for (size_t j = 0; j < obs_in_map.size(); ++j) {
            // 根據觀測值找到匹配的地圖中真實路標
            Map::single_landmark_s landmark = map_landmarks.landmark_list[obs_in_map[j].id - 1];
            // 粒子觀測值
            double x = obs_in_map.at(j).x;
            double y = obs_in_map.at(j).y;
            // 真實粒子位置
            double ux = landmark.x_f;
            double uy = landmark.y_f;
            // 計算概率P(x,y)
            double exponent = pow((x - ux), 2) / (2 * pow(std_x, 2)) + pow((y - uy), 2) / (2 * pow(std_y, 2));
            double p_xy = 1. / (2 * M_PI * std_x * std_y) * exp(-exponent);
            // 有多少個觀測路標就會連續乘以多少次權值
            particles.at(i).weight *= p_xy;
        }
        weights.push_back(particles.at(i).weight);
    }

4、重採樣resample()

計算完每一個粒子的權重後，根據權重大小，刪除權重小的粒子，同時在權重大的粒子附近拋灑新的粒子，以此保持整個粒子集的數量不變。

離散分佈函數discrete_distribution，首先根據每個粒子的權重建一個離散分佈集合，然後在這個集合上隨機採樣，權重越大，被採樣的可能性也越高。

重採樣之後，重複上面預測、座標轉換、數據關聯、更新權重的過程，就可以讓粒子羣越來越集中，所求得的最高權重粒子與真值位置越來越接近。

void ParticleFilter::resample() {

    // 離散均勻分佈d
    std::random_device rd;// 隨機數
    std::mt19937 gen(rd());
    std::discrete_distribution<> d{std::begin(weights), std::end(weights)};

    // 建立重採樣粒子集
    vector<Particle> resampled_particles(num_particles);
    for (size_t i = 0; i < num_particles; ++i) {
        resampled_particles[i] = particles[d(gen)];
    }
    particles = resampled_particles;
    
    // 權重清零
    weights.clear();
}

參考：
無人駕駛