传感器融合-Lidar and Radar Sensorfusion源码解读

1.完整代码地址:
链接：https://pan.baidu.com/s/1bV7ImmrRmee7KBQRKdQX6Q
提取码：x8hn
2.理论部分:

3.代码部分
(1)初始化(initialization)
使用Eigen库中非定长的数据结构，下图中的VerctorXd表示X维的列矩阵，其中的元素数据类型为double。

#include "Eigen/Dense"

class KalmanFilter {
public:

    KalmanFilter();
    ~KalmanFilter();
    void Initialization(Eigen::VectorXd x_in);
    bool IsInitialized();
    void SetF(Eigen::MatrixXd F_in);
    void SetP(Eigen::MatrixXd P_in);
    void SetQ(Eigen::MatrixXd Q_in);
    void SetH(Eigen::MatrixXd H_in);
    void SetR(Eigen::MatrixXd R_in);
    void Prediction();
    void KFUpdate(Eigen::VectorXd z);
    void EKFUpdate(Eigen::VectorXd z);
    Eigen::VectorXd GetX();
private:
    void CalculateJacobianMatrix();
    // flag of initialization
    bool is_initialized_;
    // state vector
    Eigen::VectorXd x_;
    // state covariance matrix
    Eigen::MatrixXd P_;
    // state transistion matrix
    Eigen::MatrixXd F_;
    // process covariance matrix
    Eigen::MatrixXd Q_;
    // measurement matrix
    Eigen::MatrixXd H_;
    // measurement covariance matrix
    Eigen::MatrixXd R_;
};

在这里，我们新建了一个KalmanFilter类，其中定义了一个叫做x_的变量，表示这个卡尔曼滤波器的状态向量。
(2)预测（Prediction）
完成初始化后，我们开始写Prediction部分的代码。首先是公式

这里的x为状态向量，通过左乘一个矩阵F，再加上外部的影响u，得到预测的状态向量x’。这里的F成为状态转移矩阵（state transistion matrix）。以2维的匀速运动为例，这里的x为

对于匀速运动模型，根据中学物理课本中的公式s1 = s0 + vt，经过时间△t后的预测状态向量应该是

由于假设当前运动为匀速运动，加速度为0，加速度不会对预测造成影响，即

如果换成加速或减速运动模型，就可以引入加速度a_x和a_y，根据s1 = s0 + vt + at^2/2这里的u会变成：

作为入门课程，这里不讨论太复杂的模型，因此公式

最终将写成

由于每次做预测时，△t的大小不固定，因此我们专门写一个函数SetF()。

void KalmanFilter::SetF(Eigen::MatrixXd F_in)
{
    F_ = F_in;
}

再看预测模块的第二个公式

该公式中P表示系统的不确定程度，这个不确定程度，在卡尔曼滤波器初始化时会很大，随着越来越多的数据注入滤波器中，不确定程度会变小，P的专业术语叫状态协方差矩阵（state covariance matrix）；这里的Q表示过程噪声（process covariance matrix），即无法用x’=Fx+u表示的噪声，比如车辆运动时突然到了上坡，这个影响是无法用之前的状态转移估计的。

以激光雷达为例。激光雷达只能测量点的位置，无法测量点的速度，因此对于激光雷达的协方差矩阵来说，对于位置信息，其测量位置较准，不确定度较低；对于速度信息，不确定度较高。因此可以认为这里的P为：

由于Q对整个系统存在影响，但又不能太确定对系统的影响有多大。工程上，我们一般将Q设置为单位矩阵参与运算，即

根据以上内容和公式

我们就可以写出预测模块的代码了

void KalmanFilter::SetF(Eigen::MatrixXd F_in)
{
    F_ = F_in;
}
void KalmanFilter::SetP(Eigen::MatrixXd P_in)
{
    P_ = P_in;
}
void KalmanFilter::SetQ(Eigen::MatrixXd Q_in)
{
    Q_ = Q_in;
}
void KalmanFilter::SetH(Eigen::MatrixXd H_in)
{
    H_ = H_in;
}
void KalmanFilter::SetR(Eigen::MatrixXd R_in)
{
    R_ = R_in;
}
void KalmanFilter::Prediction()
{
    x_ = F_ * x_;
    Eigen::MatrixXd Ft = F_.transpose();
    P_ = F_ * P_ * Ft + Q_;
}

际编程时x’及P’不需要申请新的内存去存储，使用原有的x和P代替即可。

(3)观测（Measurement）
观测的第一个公式是

这个公式计算的是实际观测到的测量值z与预测值x’之间差值y。不同传感器的测量值一般不同，比如激光雷达测量的位置信号为x方向和y方向上的距离，毫米波雷达测量的是位置和角度信息。因此需要将状态向量左乘一个矩阵H，才能与测量值进行相应的运算，这个H被称为测量矩阵（Measurement Matrix）。

激光雷达的测量值为

其中xm和ym分别表示x方向上的测量（measurement）值。
由于x’是一个41的列向量，如果要与一个21的列向量z进行减运算，需要左乘一个2*4的矩阵才行，因此整个公式最终要写成：

即，对于激光雷达来说，这里的测量矩阵H为：

求得y值后，对y值乘以一个加权量，再加到原来的预测量上去，就可以得到一个既考虑了测量值，又考虑了预测模型的位置的状态向量了。
那么y的这个权值该如何取呢？
再看接下里的两个公式


这两个公式求的是卡尔曼滤波器中一个很重要的量——卡尔曼增益K（Kalman Gain），用人话讲就是求y值的权值。
公式中的R是测量噪声矩阵（measurement covariance matrix），这个表示的是测量值与真值之间的差值。一般情况下，传感器的厂家会提供该值。S只是为了简化公式，写的一个临时变量，不要太在意。
看最后两个公式


这两个公式，实际上完成了卡尔曼滤波器的闭环，第一个公式是完成了当前状态向量x的更新，不仅考虑了上一时刻的预测值，也考虑了测量值，和整个系统的噪声，第二个公式根据卡尔曼增益，更新了系统的不确定度P，用于下一个周期的运算，该公式中的I为与状态向量同维度的单位矩阵。
将以上五个公式写成代码如下：

void KalmanFilter::KFUpdate(Eigen::VectorXd z)
{
    Eigen::VectorXd y = z - H_ * x_;
    Eigen::MatrixXd Ht = H_.transpose();
    Eigen::MatrixXd S = H_ * P_ * Ht + R_;
    Eigen::MatrixXd Si = S.inverse();
    Eigen::MatrixXd K =  P_ * Ht * Si;
    x_ = x_ + (K * y);
    int x_size = x_.size();
    Eigen::MatrixXd I = Eigen::MatrixXd::Identity(x_size, x_size);
    P_ = (I - K * H_) * P_;
}

主函数main.cpp:
需要用到的自定义数据类型

class MeasurementPackage {
public:
  long long timestamp_;
  enum SensorType{
    LASER,
    RADAR
  } sensor_type_;
  Eigen::VectorXd raw_measurements_;
};

class GroundTruthPackage {
public:
  long timestamp_;
  enum SensorType{
    LASER,
    RADAR
  } sensor_type_;
  Eigen::VectorXd gt_values_;
};

int main(int argc, char* argv[]) {

    // 设置毫米波雷达/激光雷达输入数据的路径
    // Set radar & lidar data file path
    std::string input_file_name = "../data/sample-laser-radar-measurement-data-2.txt";

    // 打开数据，若失败则输出失败信息，返回-1，并终止程序
    // Open file. if failed return -1 & end program
    std::ifstream input_file(input_file_name.c_str(), std::ifstream::in);
    if (!input_file.is_open()) {
        std::cout << "Failed to open file named : " << input_file_name << std::endl;
        return -1;
    }

    // 分配内存
    // measurement_pack_list：毫米波雷达/激光雷达实际测得的数据。数据包含测量值和时间戳，即融合算法的输入。
    // groundtruth_pack_list：每次测量时，障碍物位置的真值。对比融合算法输出和真值的差别，用于评估融合算法结果的好坏。
    std::vector<MeasurementPackage> measurement_pack_list;
    std::vector<GroundTruthPackage> groundtruth_pack_list;

    // 通过while循环将雷达测量值和真值全部读入内存，存入measurement_pack_list和groundtruth_pack_list中
    // Store radar & lidar data into memory
    std::string line;
    while (getline(input_file, line)) {
        std::string sensor_type;
        MeasurementPackage meas_package;
        GroundTruthPackage gt_package;
        std::istringstream iss(line);
        long long timestamp;

        // 读取当前行的第一个元素，L代表Lidar数据，R代表Radar数据
        // Reads first element from the current line. L stands for Lidar. R stands for Radar.
        iss >> sensor_type;
        if (sensor_type.compare("L") == 0) {
            // 激光雷达数据 Lidar data
            // 该行第二个元素为测量值x，第三个元素为测量值y，第四个元素为时间戳(纳秒）
            // 2nd element is x; 3rd element is y; 4th element is timestamp(nano second)
            meas_package.sensor_type_ = MeasurementPackage::LASER;
            meas_package.raw_measurements_ = Eigen::VectorXd(2);
            float x;
            float y;
            iss >> x;
            iss >> y;
            meas_package.raw_measurements_ << x, y;
            iss >> timestamp;
            meas_package.timestamp_ = timestamp;
            measurement_pack_list.push_back(meas_package);
        } else if (sensor_type.compare("R") == 0) {
            // 毫米波雷达数据 Radar data
            // 该行第二个元素为距离pho，第三个元素为角度phi，第四个元素为径向速度pho_dot，第五个元素为时间戳(纳秒）
            // 2nd element is pho; 3rd element is phi; 4th element is pho_dot; 5th element is timestamp(nano second)
            meas_package.sensor_type_ = MeasurementPackage::RADAR;
            meas_package.raw_measurements_ = Eigen::VectorXd(3);
            float rho;
            float phi;
            float rho_dot;
            iss >> rho;
            iss >> phi;
            iss >> rho_dot;
            meas_package.raw_measurements_ << rho, phi, rho_dot;
            iss >> timestamp;
            meas_package.timestamp_ = timestamp;
            measurement_pack_list.push_back(meas_package);
        }

        // 当前行的最后四个元素分别是x方向上的距离真值，y方向上的距离真值，x方向上的速度真值，y方向上的速度真值
        // read ground truth data to compare later
        float x_gt;
        float y_gt;
        float vx_gt;
        float vy_gt;
        iss >> x_gt;
        iss >> y_gt;
        iss >> vx_gt;
        iss >> vy_gt;
        gt_package.gt_values_ = Eigen::VectorXd(4);
        gt_package.gt_values_ << x_gt, y_gt, vx_gt, vy_gt;
        groundtruth_pack_list.push_back(gt_package);
    }

    std::cout << "Success to load data." << std::endl;

    // 开始部署跟踪算法
    SensorFusion fuser;

    for (size_t i = 0; i < measurement_pack_list.size(); ++i) {
        fuser.Process(measurement_pack_list[i]);
        Eigen::Vector4d x_out = fuser.kf_.GetX();

        std::cout << "x " << x_out(0)
                  << " y " << x_out(1)
                  << " vx " << x_out(2)
                  << " vy " << x_out(3) 
                  << std::endl;
    }
}

sensorfusion.cpp

SensorFusion::SensorFusion()
{
    is_initialized_ = false;
    last_timestamp_ = 0.0;

    // 初始化激光雷达的测量矩阵 H_lidar_
    // Set Lidar's measurement matrix H_lidar_
    H_lidar_ = Eigen::MatrixXd(2, 4);
    H_lidar_ << 1, 0, 0, 0,
                0, 1, 0, 0;

    // 设置传感器的测量噪声矩阵，一般由传感器厂商提供，如不提供，也可通过有经验的工程师调试得到
    // Set R. R is provided by Sensor supplier, in sensor datasheet
    // set measurement covariance matrix
    R_lidar_ = Eigen::MatrixXd(2, 2);
    R_lidar_ << 0.0225, 0,
                0, 0.0225;

    // Measurement covariance matrix - radar
    R_radar_ = Eigen::MatrixXd(3, 3);
    R_radar_ << 0.09, 0, 0,
                0, 0.0009, 0,
                0, 0, 0.09;
}

SensorFusion::~SensorFusion()
{

}

void SensorFusion::Process(MeasurementPackage measurement_pack)
{
    // 第一帧数据用于初始化 Kalman 滤波器
    if (!is_initialized_) {
        Eigen::Vector4d x;
        if (measurement_pack.sensor_type_ == MeasurementPackage::LASER) {
            // 如果第一帧数据是激光雷达数据，没有速度信息，因此初始化时只能传入位置，速度设置为0
            x << measurement_pack.raw_measurements_[0], measurement_pack.raw_measurements_[1], 0, 0;
        } else if (measurement_pack.sensor_type_ == MeasurementPackage::RADAR) {
            // 如果第一帧数据是毫米波雷达，可以通过三角函数算出x-y坐标系下的位置和速度
            float rho = measurement_pack.raw_measurements_[0];
            float phi = measurement_pack.raw_measurements_[1];
            float rho_dot = measurement_pack.raw_measurements_[2];
            float position_x = rho * cos(phi);
            if (position_x < 0.0001) {
                position_x = 0.0001;
            }
            float position_y = rho * sin(phi);
            if (position_y < 0.0001) {
                position_y = 0.0001;
            }
            float velocity_x = rho_dot * cos(phi);
            float velocity_y = rho_dot * sin(phi);
            x << position_x, position_y, velocity_x , velocity_y;
        }
        
        // 避免运算时，0作为被除数
        if (fabs(x(0)) < 0.001) {
            x(0) = 0.001;
        }
        if (fabs(x(1)) < 0.001) {
            x(1) = 0.001;
        }
        // 初始化Kalman滤波器
        kf_.Initialization(x);

        // 设置协方差矩阵P
        Eigen::MatrixXd P = Eigen::MatrixXd(4, 4);
        P << 1.0, 0.0, 0.0, 0.0,
             0.0, 1.0, 0.0, 0.0,
             0.0, 0.0, 1000.0, 0.0,
             0.0, 0.0, 0.0, 1000.0;
        kf_.SetP(P);

        // 设置过程噪声Q
        Eigen::MatrixXd Q = Eigen::MatrixXd(4, 4);
        Q << 1.0, 0.0, 0.0, 0.0,
             0.0, 1.0, 0.0, 0.0,
             0.0, 0.0, 1.0, 0.0,
             0.0, 0.0, 0.0, 1.0;
        kf_.SetQ(Q);

        // 存储第一帧的时间戳，供下一帧数据使用
        last_timestamp_ = measurement_pack.timestamp_;
        is_initialized_ = true;
        return;
    }

    // 求前后两帧的时间差，数据包中的时间戳单位为微秒，处以1e6，转换为秒
    double delta_t = (measurement_pack.timestamp_ - last_timestamp_) / 1000000.0; // unit : s
    last_timestamp_ = measurement_pack.timestamp_;

    // 设置状态转移矩阵F
    Eigen::MatrixXd F = Eigen::MatrixXd(4, 4);
    F << 1.0, 0.0, delta_t, 0.0,
         0.0, 1.0, 0.0, delta_t,
         0.0, 0.0, 1.0, 0.0,
         0.0, 0.0, 0.0, 1.0;
    kf_.SetF(F);

    // 预测
    kf_.Prediction();

    // 更新
    if (measurement_pack.sensor_type_ == MeasurementPackage::LASER) {
        kf_.SetH(H_lidar_);
        kf_.SetR(R_lidar_);
        kf_.KFUpdate(measurement_pack.raw_measurements_);
    } else if (measurement_pack.sensor_type_ == MeasurementPackage::RADAR) {
        kf_.SetR(R_radar_);
        // Jocobian矩阵Hj的运算已包含在EKFUpdate中
        kf_.EKFUpdate(measurement_pack.raw_measurements_);
    }
}