0. 粒子滤波流程

之前学习记录的文档，这里也拿出来分享一下~

基本原理：随机选取预测域的 N NN 个点，称为粒子。以此计算出预测值，并算出在测量域的概率，即权重，加权平均就是最优估计。之后按权重比例，重采样，进行下次迭代。

1. 初始状态：用大量粒子模拟X（t），粒子在空间内均匀分布；

2. 预测阶段：根据状态转移方程，每一个粒子得到一个预测粒子；

3. 校正阶段：对预测粒子进行评价，越接近于真实状态的粒子，其权重越大；

4. 重采样：根据粒子权重对粒子进行筛选，筛选过程中，既要大量保留权重大的粒子，又要有一小部分权重小的粒子；

5. 滤波：将重采样后的粒子带入状态转移方程得到新的预测粒子，即步骤2。

   

1. 初始状态

我们假设 GPS 的位置及航向输出服从正态分布，因此在得到 GPS 的初始输出后，我们可以根据初始值（均值 μ）和 GPS 观测不确定度（标准差 σ）构造无人车的定位初始分布，并通过对初始分布进行随机采样完成粒子集的初始化。

/**
 * @brief Initialize the particle filter.
 *
 * @param x Initial x position [m] from GPS.
 * @param y Initial y position [m] from GPS.
 * @param theta Initial heading angle [rad] from GPS.
 * @param std_pos Array of dimension 3 [standard deviation of x [m],
 *   standard deviation of y [m], standard deviation of theta [rad]]
 */
void ParticleFilter::Init(const double &x, const double &y, const double &theta,
                          const double std_pos[])
{
    if (!IsInited())
    {
        // create normal distributions around the initial gps measurement values
        std::default_random_engine gen;
        std::normal_distribution<double> norm_dist_x(x, std_pos[0]);
        std::normal_distribution<double> norm_dist_y(y, std_pos[1]);
        std::normal_distribution<double> norm_dist_theta(theta, std_pos[2]);

        // initialize particles one by one
        for (size_t i = 0; i < n_p; ++i)
        {
            particles(0, i) = norm_dist_x(gen);
            particles(1, i) = norm_dist_y(gen);
            particles(2, i) = norm_dist_theta(gen);
        }

        // initialize weights to 1 / n_p
        weights_nonnormalized.fill(1 / n_p);
        weights_normalized.fill(1 / n_p);

        is_inited = true;
    }
}

有几点说明如下：

• 这里使用了 C++ 的随机数引擎 std::default_random_engine 和正态分布模板类 std::normal_distribution 实现了高斯随机数的生成
• particles 是 $3\times n_p$ 的粒子 Eigen 矩阵，$n_p$ 表示粒子数目，我们在构造函数的初始值列表中将其初始化为了 $1000$，矩阵的每一列表示一个粒子的状态，矩阵的第 $0$、$1$、$2$ 行分别表示粒子的横向位置 $x$、纵向位置 $y$ 和航向角 $theta$
• weights_nonnormalized 和 weights_normalized 分别表示未归一化和归一化的重要性权重，数据类型都是 $n_p\times 1$ 的 Eigen 向量。很多粒子滤波教程中使用同一个变量存放未归一化和归一化的重要性权重，这样也是可以的，这里我们的目的是使代码逻辑更加清晰。

2. 预测阶段

根据机器人的车轮运动速度或者里程对粒子进行状态转移，即将粒子的信息带入机器人的运动模型中，加入控制噪声并产生新的粒子。

在预测步中，我们需要根据无人车的运动模型、车速、航向角速率、相邻两帧的时间间隔等将上一步的粒子集向当前时刻进行预测。这里我们我们假设自车遵从 CRTV 运动模型，关于 CRTV，在此前文章《从贝叶斯滤波到无迹卡尔曼滤波》中我们已经介绍过，不再赘述，这里我们这里直接给出不计噪声时的 CRTV 状态方程

式 (5.1) 中，$\omega$ 即自车的航向角速率。CTRV 是 CV 的一般形式，当 $\omega =0$ 时，CTRV 退化为 CV。

/**
 * @brief Predict new state of particle according to the system motion model.
 *
 * @param velocity Velocity of car [m/s]
 * @param yaw_rate Yaw rate of car [rad/s]
 * @param delta_t delta time between last timestamp and current timestamp [s]
 * @param std_pos Array of dimension 3 [standard deviation of x [m],
 *   standard deviation of y [m], standard deviation of yaw [rad]]
 */
void ParticleFilter::Predict(const double &velocity, const double &yaw_rate,
                             const double &delta_t, const double std_pos[])
{
    if (!IsInited())
        return;

    // create process noise's normal distributions of which the mean is zero
    std::default_random_engine gen;
    std::normal_distribution<double> norm_dist_x(0, std_pos[0]);
    std::normal_distribution<double> norm_dist_y(0, std_pos[1]);
    std::normal_distribution<double> norm_dist_theta(0, std_pos[2]);

    // predict state of particles one by one
    for (size_t i = 0; i < n_p; ++i)
    {
        double theta_last = particles(2, i);

        Eigen::Vector3d state_trans_item_motion;
        Eigen::Vector3d state_trans_item_noise;

        state_trans_item_noise << norm_dist_x(gen), norm_dist_y(gen), norm_dist_theta(gen);

        if (std::fabs(yaw_rate) > 0.001) // CTRV model
        {
            state_trans_item_motion << velocity / yaw_rate * (sin(theta_last + yaw_rate * delta_t) - sin(theta_last)),
                velocity / yaw_rate * (-cos(theta_last + yaw_rate * delta_t) + cos(theta_last)),
                yaw_rate * delta_t;
        }
        else // approximate CV model
        {
            state_trans_item_motion << velocity * cos(theta_last) * delta_t,
                velocity * sin(theta_last) * delta_t,
                yaw_rate * delta_t;
        }

        // predict new state of the ith particle
        particles.col(i) = particles.col(i) + state_trans_item_motion + state_trans_item_noise;

        // normalize theta
        NormalizeAngle(particles(2, i));
    }
}

状态转移过程中的过程噪声我们假设为零均值的高斯白噪声。很明显预测步只改变了每个粒子的状态，未改变粒子的权重。每个粒子的预测航向角我们都做了 $[-\pi ,\pi ]$ 的归一化处理，后面在计算系统最终的加权状态估计时不需要重复处理。

粒子滤波基于贝叶斯滤波框架。在贝叶斯滤波中，我们试图估计系统的状态（状态变量）$x_t$，其中$t$表示时间步。贝叶斯滤波的核心思想是使用贝叶斯定理来更新状态的后验概率分布，即$P(x_t|z_{1:t},u_{1:t})$，其中$z_{1:t}$表示观测序列，$u_{1:t}$表示控制输入序列。

在粒子滤波中$x_t$是在时刻$t$的状态，$u_t$是时刻$t$的控制输入，$w_t$是过程噪声，表示系统模型中的不确定性

3. 校正阶段

这里面其实涉及到几个步骤，其实主要起到的作用是更新+粒子权重更新的部分。

更新步的目的是根据最新的路标观测结果（自车局部坐标系下的横纵向相对位置），更新预测步后每个粒子的重要性权重。更新步主要由以下四个子步骤组成，需要对粒子集中的每个粒子依次执行以下步骤，我们结合代码进行阐述。

步骤 (1)： 坐标变换

无人车实时观测到的路标结果基于自车局部坐标系，我们将其转换到地图的全局坐标系，关于坐标系变换推导并不复杂，可见参考 34。假设当前时刻自车观测到某个路标 $lmrk(x_c,y_c)$，下角标 $c$ 表示自车坐标系，该路标对应于地图坐标系中的位置为 $lmrk(x_m,y_m)$，下角标 $m$ 表示地图坐标系。对于粒子 $p(x_p,y_p,{\theta }_p)$，下角标 $p$ 表示粒子，我们直接给出从 $lmrk(x_c,y_c)$ 到 $lmrk(x_m,y_m)$ 的坐标变换方程。

/**
 * @brief Transform observed landmarks from local ego vehicle coordinate to
 *   global map coordinate.
 *
 * @param lmrks_obs Observed landmarks in ego vehicle coordinate.
 * @param particle Single particle with state of [x, y, theta]
 * @param lmrks_trans2map Observed landmarks transformed from local ego vehicle
 *   coordinate to global map coordinate.
 */
void ParticleFilter::TransLandmarksFromVehicle2Map(const std::vector<LandMark_Obs> &lmrks_obs,
                                                   const Eigen::Vector3d &particle,
                                                   std::vector<LandMark_Map> &lmrks_trans2map)
{
    for (size_t i = 0; i < lmrks_obs.size(); ++i)
    {
        lmrks_trans2map[i].x = lmrks_obs[i].x * cos(particle(2)) -
                               lmrks_obs[i].y * sin(particle(2)) + particle(0);

        lmrks_trans2map[i].y = lmrks_obs[i].x * sin(particle(2)) +
                               lmrks_obs[i].y * cos(particle(2)) + particle(1);
    }
}

这一部分其实就是构建观测模型，其描述了如何将系统状态映射到观测值。通常，这可以用一个非线性函数来表示。其中，$z_t$是在时刻$t$的观测值，$v_t$是观测噪声，表示观测模型中的不确定性。如果是深度学习给到的分割结果，这里可以直接用输入数据（如上）

步骤 (2)： 查找传感器感知范围内的地图路标

传感器的实际感知范围是有限的，我们需要找到每个粒子对应的传感器感知范围内的地图路标。

/**
 * @brief Find map landmarks within the sensor measuremet range.
 *
 * @param lmrks_map All map landmarks.
 * @param particle Single particle with state of [x, y, theta]
 * @param snsr_range Sensor measuremet range.
 * @param lmrks_within_range Map landmarks within the sensor measuremet range.
 */
void ParticleFilter::FindMapLandmarksWithinSensorRange(const std::vector<LandMark_Map> &lmrks_map,
                                                       const Eigen::Vector3d &particle,
                                                       const double &snsr_range,
                                                       std::vector<LandMark_Map> &lmrks_within_range)
{
    static double distance_threshold_square = snsr_range * snsr_range;

    for (auto landmark : lmrks_map)
    {
        double distance_square = std::pow(particle(0) - landmark.x, 2) +
                                 std::pow(particle(1) - landmark.y, 2);

        if (distance_square <= distance_threshold_square)
            lmrks_within_range.push_back(landmark);
    }
}

步骤 (1) 和步骤 (2) 作为步骤 (3) 的输入，其顺序无关紧要。

步骤 (3)： 数据关联

数据关联的目的是找到观测路标与实际地图路标的一一对应关系，步骤 (4) 中需要通过这个对应关系更新每个粒子的权重。这里我们使用一种最为简单的数据关联方法——最近邻（Nearest Neighbor，NN）数据关联，其核心思想很直观：对于两个待关联的数据集，数据间的欧氏距离越小，关联的概率越高。NN 数据关联方法的优缺点总结如下（图片出自 Udacity）。

…详情请参照古月居