有了上一節分別對激光雷達做KF、對毫米波雷達做EKF兩種流程之後，我們合併KF和EKF的跟蹤算法的代碼，將他們封裝在一個名爲KalmanFilter的類中，方便後續調用。下面做一個多傳感器融合定位實驗，採集到的數據爲兩種交替出現的數據。
代碼整體框架如圖所示：

前言

無論是KF處理線性問題還是EKF處理非線性問題，它都只涉及單一傳感器的障礙物跟蹤。激光雷達測量位置精度更高但是無法測量速度，毫米波雷達測量速度準確，但是在位置測量精度上低。爲了充分利用各個傳感器的優勢，需要做多傳感器融合定位。

激光雷達Lidar：發射激光束，利用光的直線傳播能夠直接獲得障礙物在笛卡爾座標系下x、y、z方向上距離。建立三維點雲圖，探測目標的位置、速度等特徵量。
狀態向量分別是位置和速度的x、y方向（x,y,vx,vy）

毫米波雷達Radar：發射毫米波（電磁波），利用多普勒效應，所測數據都是在極座標系下。毫米波導引頭穿透霧、煙、灰塵的能力強，成本低，但是探測距離受到頻段損耗的直接制約。
狀態向量爲極座標系下雷達的距離ρ，方向角ϕ和距離的變化率（徑向速度）ρ’。

對比：
Lidar探測精度高、探測範圍廣，穩定性強，成本高。但是在雨雪霧等極端天氣下、光束受遮擋後性能較差，採集的數據量過大。64線70萬元人民幣。

Radar穿透霧、煙、灰塵的能力強，探測距離受到頻段損耗的直接制約（想要探測的遠，就必須使用高頻段雷達），也無法感知行人，並且對周邊所有障礙物無法進行精準的建模。100美元左右。

一、輸入數據分析

在data目錄下名爲sample-laser-radar-measurement-data-2.txt的文件，這就是傳感器融合的輸入數據。
假設：Lidar和Radar都固定在原點（0,0）處，且檢測障礙物頻率相同，兩種雷達交替觸發檢測。

每行第一個字母L是Lidar，R是Radar。
第1、2行表示原點位置所以都只有時間戳。
Lidar：X方向測量值、Y方向測量值、時間戳、X方向位置真值、Y方向位置真值、X方向速度真值、Y方向速度真值。
Radar：障礙物在極座標系下的距離ρ、角度ϕ、徑向速度ρ’、時間戳、X方向位置真值、Y方向位置真值、X方向速度真值、Y方向速度真值。

主函數main()

框架爲：

這部分我們重點考慮填充測量數據和真值數據，把傳感器輸入數據sample-laser-radar-measurement-data-2.txt填充進來。

1、導入txt文件

這部分可以說是從txt文件中導入數據的標配了。

// 設置毫米波雷達/激光雷達輸入數據的路徑
    // 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;
    }

2、測量值類和真值類

測量值MeasurementPackage類：時間戳、傳感器類型、位置（xy）

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

真值GroundTruthPackage類：時間戳、傳感器類型、數據(距離、角度、鏡像速度)

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

3、txt數據導入測量值類和真值類

值得注意的是，未來主要用的數據是測量數據，真值是爲了做對比才出現的，作用僅限於最後用於畫圖。我們使用測量數據進行process（）函數，如果是Lidar數據就用KF更新，如果是Radar數據就用EKF更新。

   std::vector<MeasurementPackage> measurement_pack_list;
    std::vector<GroundTruthPackage> groundtruth_pack_list;

    std::string line;
    while (getline(input_file, line)) {// getline後是string類型
        std::string sensor_type;
        MeasurementPackage meas_package;
        GroundTruthPackage gt_package;
        std::istringstream iss(line);// 從string轉爲數據流格式
        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) {// 相等輸出0
            // 激光雷達數據 Lidar data
            // 測量值：x，y，時間戳
            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，時間戳
            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，vx，vy
        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類

先來看SensorFusion類的構成，主函數是在process()中，通過輸入測量數據不斷進行處理，最終得到（x,y,vx,vy）

class SensorFusion {
public:
    SensorFusion();
    ~SensorFusion();
    // 主函數process（）
    void Process(MeasurementPackage measurement_pack);
    KalmanFilter kf_;

private:
    bool is_initialized_;// 是否初始化
    long last_timestamp_;// 上一次時間戳
    // 傳感器測量噪聲R
    Eigen::MatrixXd R_lidar_;//2*2
    Eigen::MatrixXd R_radar_;//3*3
    // 測量矩陣H
    Eigen::MatrixXd H_lidar_;//2*4
};

1、構造函數

構造函數，第一幀初始化H_lidar,R_lidar,R_radar。

SensorFusion::SensorFusion()
{
    // 1.第一幀還沒初始化，上次時間戳爲0
    is_initialized_ = false;
    last_timestamp_ = 0.0;

    // 2.初始化激光雷達的測量矩陣 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;

    // 3.設置兩種雷達的測量噪聲矩陣R，一般由傳感器廠商提供，如不提供，也可通過有經驗的工程師調試得到
    // 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;
}

2、process()函數

首先讀入傳感器數據，如果是第一次讀入，則需要對卡爾曼濾波器的各個矩陣進行初始化操作（初始化狀態x，協方差矩陣P、過程噪聲Q）；如果不是第一次讀入，證明卡爾曼濾波器已完成初始化，先進行狀態預測（根據兩幀時間差給狀態轉移矩陣F賦值、計算出新的狀態x和協方差矩陣P），接下來進行測量更新的步驟（注意EKF不需要測量矩陣H，只需要測量噪聲R即可）；最後輸出融合後的障礙物位置、速度。

Udacity學城上對這一過程有更詳細的描述。

void SensorFusion::Process(MeasurementPackage measurement_pack)
{
    // 1.第一幀數據用於初始化 Kalman 濾波器
    if (!is_initialized_) {
        Eigen::Vector4d x;

        // 1.1 激光雷達Lidar初始化(x,y,vx,vy)
        // 第一幀沒有速度信息，因此初始化時只能傳入位置，速度設置爲0
        if (measurement_pack.sensor_type_ == MeasurementPackage::LASER) {
            x << measurement_pack.raw_measurements_[0], measurement_pack.raw_measurements_[1], 0, 0;
        }
        // 1.2 毫米波雷達初始化(x,y,vx,vy)
        // 通過三角函數算出x-y座標系下的位置和速度
         else if (measurement_pack.sensor_type_ == MeasurementPackage::RADAR) {
            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;
        }
        // 1.3 初始化Kalman濾波器
        kf_.Initialization(x);

        // 1.4 設置協方差矩陣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);

        // 1.5 設置過程噪聲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);

        // 1.6 更新時間戳爲當前時間戳，初始化完成
        last_timestamp_ = measurement_pack.timestamp_;
        is_initialized_ = true;
        return;
    }
    // 2. 狀態轉移矩陣F賦值
    // 求前後兩幀的時間差，數據包中的時間戳單位爲微秒，處以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);

    // 3.預測
    kf_.Prediction();

    // 4.更新
    // 4.1 激光雷達，KF更新
    if (measurement_pack.sensor_type_ == MeasurementPackage::LASER) {
        kf_.SetH(H_lidar_);
        kf_.SetR(R_lidar_);
        kf_.KFUpdate(measurement_pack.raw_measurements_);
    } 
    // 4.2 毫米波雷達，EKF更新
    else if (measurement_pack.sensor_type_ == MeasurementPackage::RADAR) {
        kf_.SetR(R_radar_);
        // Jocobian矩陣Hj的運算已包含在EKFUpdate中
        kf_.EKFUpdate(measurement_pack.raw_measurements_);
    }
}

3、結果

將融合結果與真值繪製在同一座標系下，即可看到融合的實際效果

這裏實現的多傳感器融合更多像是一幀按照Lidar數據做KF，下一幀用Radar做EKF，但是並沒有對兩種數據做取捨之類的，就是一個接一下的來。

如下圖所示，在k+1時刻收到激光雷達數據時，根據k時刻的狀態完成一次預測，再根據k+1時刻的激光雷達的觀測數據實現測量值更新（KFUpdate）；在k+2時刻收到毫米波雷達的數據時，根據k+1時刻的狀態完成一次預測，再根據k+2時刻的毫米波雷達的觀測數據實現測量值更新（EKFUpdate）。

多傳感器融合定位2（封裝KF和EKF）

前言

一、輸入數據分析

主函數main()

1、導入txt文件

2、測量值類和真值類

3、txt數據導入測量值類和真值類

二、SensorFusion類

1、構造函數

2、process()函數

3、結果

推薦2款開源、美觀的WinForm UI控件庫

NET9 AspnetCore將整合OpenAPI的文檔生成功能而無需三方庫

VINS-Mono 代碼詳細解讀——初始化1：視覺SFM詳解 processImage（）+initialStructure（）

多傳感器融合定位2（封裝KF和EKF）

Endnote連接Word自動插入畢業論文參考文獻

《wiki官網教程》2 編寫簡單的服務器service和客戶端 client(C++)

VINS-Mono 代碼詳細解讀——基於滑動窗口的緊耦合後端非線性優化 optimization()+滑動窗slideWindow()

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結